265 lines
5.6 KiB
C
265 lines
5.6 KiB
C
|
|
#include "stm32f1xx_hal.h"
|
|
#include "defines.h"
|
|
#include "setup.h"
|
|
#include "config.h"
|
|
|
|
|
|
volatile int posl = 0;
|
|
volatile int posr = 0;
|
|
volatile int pwml = 0;
|
|
volatile int pwmr = 0;
|
|
float pwmrl = 0;
|
|
|
|
extern volatile adc_buf_t adc_buffer;
|
|
|
|
extern volatile uint32_t timeout;
|
|
|
|
uint8_t buzzerFreq = 0;
|
|
uint8_t buzzerPattern = 0;
|
|
|
|
uint8_t enable = 0;
|
|
|
|
const int pwm_res = 64000000 / 2 / PWM_FREQ; // = 2000
|
|
|
|
const uint8_t hall_to_pos[8] = {
|
|
0,
|
|
0,
|
|
2,
|
|
1,
|
|
4,
|
|
5,
|
|
3,
|
|
0,
|
|
};
|
|
|
|
inline void blockPWM(int pwm, int pos, int *u, int *v, int *w) {
|
|
switch(pos) {
|
|
case 0:
|
|
*u = 0;
|
|
*v = pwm;
|
|
*w = -pwm;
|
|
break;
|
|
case 1:
|
|
*u = -pwm;
|
|
*v = pwm;
|
|
*w = 0;
|
|
break;
|
|
case 2:
|
|
*u = -pwm;
|
|
*v = 0;
|
|
*w = pwm;
|
|
break;
|
|
case 3:
|
|
*u = 0;
|
|
*v = -pwm;
|
|
*w = pwm;
|
|
break;
|
|
case 4:
|
|
*u = pwm;
|
|
*v = -pwm;
|
|
*w = 0;
|
|
break;
|
|
case 5:
|
|
*u = pwm;
|
|
*v = 0;
|
|
*w = -pwm;
|
|
break;
|
|
default:
|
|
*u = 0;
|
|
*v = 0;
|
|
*w = 0;
|
|
}
|
|
}
|
|
|
|
inline void blockPhaseCurrent(int pos, int u, int v, int *q) {
|
|
switch(pos) {
|
|
case 0:
|
|
*q = u - v;
|
|
// *u = 0;
|
|
// *v = pwm;
|
|
// *w = -pwm;
|
|
break;
|
|
case 1:
|
|
*q = u;
|
|
// *u = -pwm;
|
|
// *v = pwm;
|
|
// *w = 0;
|
|
break;
|
|
case 2:
|
|
*q = u;
|
|
// *u = -pwm;
|
|
// *v = 0;
|
|
// *w = pwm;
|
|
break;
|
|
case 3:
|
|
*q = v;
|
|
// *u = 0;
|
|
// *v = -pwm;
|
|
// *w = pwm;
|
|
break;
|
|
case 4:
|
|
*q = v;
|
|
// *u = pwm;
|
|
// *v = -pwm;
|
|
// *w = 0;
|
|
break;
|
|
case 5:
|
|
*q = -(u - v);
|
|
// *u = pwm;
|
|
// *v = 0;
|
|
// *w = -pwm;
|
|
break;
|
|
default:
|
|
*q = 0;
|
|
// *u = 0;
|
|
// *v = 0;
|
|
// *w = 0;
|
|
}
|
|
}
|
|
|
|
uint16_t buzzerTimer = 0;
|
|
|
|
int offsetcount = 0;
|
|
int offsetrl1 = 2000;
|
|
int offsetrl2 = 2000;
|
|
int offsetrr1 = 2000;
|
|
int offsetrr2 = 2000;
|
|
int offsetdcl = 2000;
|
|
int offsetdcr = 2000;
|
|
|
|
float batteryVoltage;
|
|
float adccmd1;
|
|
float adccmd2;
|
|
|
|
int curl = 0;
|
|
// int errorl = 0;
|
|
// int kp = 5;
|
|
// volatile int cmdl = 0;
|
|
|
|
int last_pos = 0;
|
|
int timer = 0;
|
|
const int max_time = PWM_FREQ / 10;
|
|
volatile int vel = 0;
|
|
|
|
void DMA1_Channel1_IRQHandler() {
|
|
DMA1->IFCR = DMA_IFCR_CTCIF1;
|
|
// HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
|
|
|
|
if(offsetcount < 1000) { // calibrate ADC offsets
|
|
offsetcount++;
|
|
offsetrl1 = (adc_buffer.rl1 + offsetrl1) / 2;
|
|
offsetrl2 = (adc_buffer.rl2 + offsetrl2) / 2;
|
|
offsetrr1 = (adc_buffer.rr1 + offsetrr1) / 2;
|
|
offsetrr2 = (adc_buffer.rr2 + offsetrr2) / 2;
|
|
offsetdcl = (adc_buffer.dcl + offsetdcl) / 2;
|
|
offsetdcr = (adc_buffer.dcr + offsetdcr) / 2;
|
|
return;
|
|
}
|
|
|
|
batteryVoltage = batteryVoltage * 0.999 + ((float)adc_buffer.batt1 * ADC_BATTERY_VOLT) * 0.001;
|
|
adccmd1 = adccmd1 * 0.999 + (float)adc_buffer.l_rx2 * 0.001;
|
|
adccmd2 = adccmd2 * 0.999 + (float)adc_buffer.l_tx2 * 0.001;
|
|
|
|
//0-4096
|
|
//+-2000
|
|
|
|
pwmrl = 0;
|
|
if(adccmd1 - 700 > 0){
|
|
pwmrl += adccmd1 - 700;
|
|
}
|
|
if(adccmd2 - 700 > 0){
|
|
pwmrl -= adccmd2 - 700;
|
|
}
|
|
|
|
pwmrl = powf((pwmrl/4), 3) / 614125;
|
|
pwml = -pwmrl;
|
|
pwmr = pwmrl;
|
|
|
|
|
|
|
|
if(ABS((adc_buffer.dcl - offsetdcl) * MOTOR_AMP_CONV_DC_AMP) > DC_CUR_LIMIT || timeout > 50 || enable == 0) {
|
|
LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
|
|
//HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
|
|
} else {
|
|
LEFT_TIM->BDTR |= TIM_BDTR_MOE;
|
|
//HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0);
|
|
}
|
|
|
|
if(ABS((adc_buffer.dcr - offsetdcr) * MOTOR_AMP_CONV_DC_AMP) > DC_CUR_LIMIT || timeout > 50 || enable == 0) {
|
|
RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;
|
|
} else {
|
|
RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
|
|
}
|
|
|
|
int ul, vl, wl;
|
|
int ur, vr, wr;
|
|
|
|
uint8_t hall_ul = !(LEFT_HALL_U_PORT->IDR & LEFT_HALL_U_PIN);
|
|
uint8_t hall_vl = !(LEFT_HALL_V_PORT->IDR & LEFT_HALL_V_PIN);
|
|
uint8_t hall_wl = !(LEFT_HALL_W_PORT->IDR & LEFT_HALL_W_PIN);
|
|
|
|
uint8_t hall_ur = !(RIGHT_HALL_U_PORT->IDR & RIGHT_HALL_U_PIN);
|
|
uint8_t hall_vr = !(RIGHT_HALL_V_PORT->IDR & RIGHT_HALL_V_PIN);
|
|
uint8_t hall_wr = !(RIGHT_HALL_W_PORT->IDR & RIGHT_HALL_W_PIN);
|
|
|
|
uint8_t halll = hall_ul * 1 + hall_vl * 2 + hall_wl * 4;
|
|
posl = hall_to_pos[halll];
|
|
posl += 2;
|
|
posl %= 6;
|
|
|
|
uint8_t hallr = hall_ur * 1 + hall_vr * 2 + hall_wr * 4;
|
|
posr = hall_to_pos[hallr];
|
|
posr += 2;
|
|
posr %= 6;
|
|
|
|
blockPhaseCurrent(posl, adc_buffer.rl1 - offsetrl1, adc_buffer.rl2 - offsetrl2, &curl);
|
|
|
|
setScopeChannel(2, (adc_buffer.rl1 - offsetrl1) / 8);
|
|
setScopeChannel(3, (adc_buffer.rl2 - offsetrl2) / 8);
|
|
|
|
buzzerTimer++;
|
|
|
|
if (buzzerFreq != 0 && (buzzerTimer / 1500) % (buzzerPattern + 1) == 0) {
|
|
if (buzzerTimer % buzzerFreq == 0) {
|
|
HAL_GPIO_TogglePin(BUZZER_PORT, BUZZER_PIN);
|
|
}
|
|
} else {
|
|
HAL_GPIO_WritePin(BUZZER_PORT, BUZZER_PIN, 0);
|
|
}
|
|
|
|
// //measure vel
|
|
// timer++;
|
|
// if(timer > max_time){
|
|
// timer = max_time;
|
|
// vel = 0;
|
|
// }
|
|
|
|
// if(posl != last_pos){
|
|
// vel = 1000 * PWM_FREQ / timer / P / 6 * 2;
|
|
|
|
// if((posl - last_pos + 6) % 6 > 2){
|
|
// vel = -vel;
|
|
// }
|
|
|
|
// timer = 0;
|
|
// }
|
|
// last_pos = posl;
|
|
|
|
//YOLOTEST
|
|
// errorl = cmdl - curl;
|
|
// pwml = kp * errorl;
|
|
|
|
blockPWM(pwml, posl, &ul, &vl, &wl);
|
|
blockPWM(pwmr, posr, &ur, &vr, &wr);
|
|
|
|
LEFT_TIM->LEFT_TIM_U = CLAMP(ul + pwm_res / 2, 10, pwm_res-10);
|
|
LEFT_TIM->LEFT_TIM_V = CLAMP(vl + pwm_res / 2, 10, pwm_res-10);
|
|
LEFT_TIM->LEFT_TIM_W = CLAMP(wl + pwm_res / 2, 10, pwm_res-10);
|
|
|
|
RIGHT_TIM->RIGHT_TIM_U = CLAMP(ur + pwm_res / 2, 10, pwm_res-10);
|
|
RIGHT_TIM->RIGHT_TIM_V = CLAMP(vr + pwm_res / 2, 10, pwm_res-10);
|
|
RIGHT_TIM->RIGHT_TIM_W = CLAMP(wr + pwm_res / 2, 10, pwm_res-10);
|
|
// HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0);blockPhaseCurrent
|
|
}
|