EFeru/hoverboard-firmware-hack-SIN
1
0
Fork 0
forked from lucysrausch/hoverboard-firmware-hack
Code Pull Requests Activity

current feedback "working"

Browse Source
This commit is contained in:
Rene Hopf
2018-01-07 03:11:55 +01:00
parent 61aff94a26
commit 86c6106fb7
3 changed files with 150 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

139
Src/main.c
View File

@@ -87,55 +87,102 @@ inline void block(int pwm, int pos, int *u, int *v, int *w) {
}
}
inline void block2(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;
}
}
int last_pos = 0;
int timer = 0;
int max_time = PWM_FREQ / 10;
volatile int vel = 0;
int offsetcount = 0;
int offsetrl1 = 2000;
int offsetrl2 = 2000;
int offsetrr1 = 2000;
int offsetrr2 = 2000;
int offsetdcl = 2000;
int offsetdcr = 2000;
volatile uint8_t uart_buf[10];
int curl = 0;
// int errorl = 0;
// int kp = 5;
// volatile int cmdl = 0;
void DMA1_Channel1_IRQHandler() {
DMA1->IFCR = DMA_IFCR_CTCIF1;
HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
/*
uart_buf[0] = 0xff;
uart_buf[1] = adc_buffer.r_dc1 - 1850 + 127;
uart_buf[2] = adc_buffer.l_dc2 - 1850 + 127;
uart_buf[3] = 127;//adc_buffer.rr1 - 2000 + 127;
uart_buf[4] = 127;//adc_buffer.rr2 - 2000 + 127;
uart_buf[5] = 127;//adc_buffer.rl1 - 2000 + 127;
uart_buf[6] = 127;//adc_buffer.rl2 - 2000 + 127;
uart_buf[7] = adc_buffer.batt1 - 1550 + 127;
uart_buf[8] = adc_buffer.bat1 - 1550 + 127;
uart_buf[9] = '\n';
// HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
if(DMA1_Channel2->CNDTR == 0){
DMA1_Channel2->CCR &= ~DMA_CCR_EN;
DMA1_Channel2->CNDTR = 10;
DMA1_Channel2->CMAR = (uint32_t)uart_buf;
DMA1_Channel2->CCR |= DMA_CCR_EN;
if(offsetcount < 1000) {
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;
}
*/
if(adc_buffer.l_dc2 > 1950) {
if(adc_buffer.dcl - offsetdcl > 40) {
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(adc_buffer.r_dc1 > 1950) {
if(adc_buffer.dcr - offsetdcr > 40) {
RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;
} else {
RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
}
int ul = 0;
int vl = 0;
int wl = 0;
int ur = 0;
int vr = 0;
int wr = 0;
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);
@@ -155,6 +202,25 @@ void DMA1_Channel1_IRQHandler() {
posr += 2;
posr %= 6;
block2(posl, adc_buffer.rl1 - offsetrl1, adc_buffer.rl2 - offsetrl2, &curl);
uart_buf[0] = 0xff;
uart_buf[1] = 127; //adc_buffer.dcl - 1850 + 127;
uart_buf[2] = 127; //adc_buffer.dcr - 1850 + 127;
uart_buf[3] = 127; ////CLAMP((adc_buffer.rr1 - offsetrr1) / 8 + 127,0,255);
uart_buf[4] = 127; ////CLAMP((adc_buffer.rr2 - offsetrr2) / 8 + 127,0,255);
uart_buf[5] = CLAMP((adc_buffer.rl1 - offsetrl1) / 8 + 127, 0, 255);
uart_buf[6] = CLAMP((adc_buffer.rl2 - offsetrl2) / 8 + 127, 0, 255);
uart_buf[7] = 127; //CLAMP(curl / 8 + 127,0,255);//adc_buffer.batt1 - 1550 + 127;
uart_buf[8] = 127 + posl * 20; //adc_buffer.bat1 - 1550 + 127;
uart_buf[9] = '\n';
if(DMA1_Channel2->CNDTR == 0) {
DMA1_Channel2->CCR &= ~DMA_CCR_EN;
DMA1_Channel2->CNDTR = 10;
DMA1_Channel2->CMAR = (uint32_t)uart_buf;
DMA1_Channel2->CCR |= DMA_CCR_EN;
}
timer++;
// if(timer > max_time){
@@ -173,17 +239,21 @@ void DMA1_Channel1_IRQHandler() {
// }
// last_pos = pos;
//YOLOTEST
// errorl = cmdl - curl;
// pwml = kp * errorl;
block(pwml, posl, &ul, &vl, &wl);
block(pwmr, posr, &ur, &vr, &wr);
LEFT_TIM->LEFT_TIM_U = CLAMP(ul + pwm_res / 2, 0, pwm_res);
LEFT_TIM->LEFT_TIM_V = CLAMP(vl + pwm_res / 2, 0, pwm_res);
LEFT_TIM->LEFT_TIM_W = CLAMP(wl + pwm_res / 2, 0, pwm_res);
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, 0, pwm_res);
RIGHT_TIM->RIGHT_TIM_V = CLAMP(vr + pwm_res / 2, 0, pwm_res);
RIGHT_TIM->RIGHT_TIM_W = CLAMP(wr + pwm_res / 2, 0, pwm_res);
HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0);
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);
}
int milli_vel_error_sum = 0;
@@ -233,6 +303,7 @@ int main(void) {
// milli_vel_error_sum += milli_vel_error;
// milli_vel_error_sum = CLAMP(milli_vel_error_sum, -200000, 200000);
// pwm = CLAMP(milli_vel_cmd / 5 + milli_vel_error_sum / 200, -500, 500);
// cmdl = 70;
pwml = 150;
pwmr = 150;