lucysrausch/hoverboard-firmware-hack
1
0
Fork 1
mirror of https://github.com/lucysrausch/hoverboard-firmware-hack.git synced 2025-08-01 08:34:27 +02:00
Code Issues Packages Projects Releases Wiki Activity

refactored code, moved user-relevant defines to config.h

Browse Source
This commit is contained in:
Niklas Fauth
2018-02-09 08:53:25 +01:00
parent 86c6106fb7
commit 94907b91cf
9 changed files with 1099 additions and 250 deletions
Download Patch File Download Diff File Expand all files Collapse all files

218
Src/bldc.c Normal file
View File

@@ -0,0 +1,218 @@
#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;
extern volatile adc_buf_t adc_buffer;
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;
}
}
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;
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);
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;
}
if((adc_buffer.dcl - offsetdcl) * MOTOR_AMP_CONV_DC_AMP > DC_CUR_LIMIT) {
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.dcr - offsetdcr) * MOTOR_AMP_CONV_DC_AMP > DC_CUR_LIMIT) {
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);
consoleScope(0, 0, (adc_buffer.rl1 - offsetrl1) / 8, (adc_buffer.rl2 - offsetrl1) / 8, 0, 0, 0, 0);
timer++;
// if(timer > max_time){
// timer = max_time;
// vel = 0;
// }
// if(pos != last_pos){
// vel = 1000 * PWM_FREQ / timer / P / 6 * 2;
// if((pos - last_pos + 6) % 6 > 2){
// vel = -vel;
// }
// timer = 0;
// }
// last_pos = pos;
//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
}

50
Src/comms.c Normal file
View File

@@ -0,0 +1,50 @@
#include "stm32f1xx_hal.h"
#include "defines.h"
#include "setup.h"
#include "config.h"
#include "stdio.h"
#include "string.h"
UART_HandleTypeDef huart2;
volatile uint8_t uart_buf[300];
//volatile char char_buf[300];
void consoleScope(int16_t ch0, int16_t ch1, int16_t ch2, int16_t ch3, int16_t ch4, int16_t ch5, int16_t ch6, int16_t ch7) {
#ifdef DEBUG_SERIAL_SERVOTERM
uart_buf[0] = 0xff;
uart_buf[1] = CLAMP(ch0+127, 0, 255);
uart_buf[2] = CLAMP(ch1+127, 0, 255);
uart_buf[3] = CLAMP(ch2+127, 0, 255);
uart_buf[4] = CLAMP(ch3+127, 0, 255);
uart_buf[5] = CLAMP(ch4+127, 0, 255);
uart_buf[6] = CLAMP(ch5+127, 0, 255);
uart_buf[7] = CLAMP(ch6+127, 0, 255);
uart_buf[8] = CLAMP(ch7+127, 0, 255);
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;
}
#endif
#ifdef DEBUG_SERIAL_ASCII
memset(&uart_buf, 0, sizeof(uart_buf));
sprintf(uart_buf, "%i;%i;%i;%i;%i;%i;%i;%i\n\r", ch0, ch1, ch2, ch3, ch4, ch5, ch6, ch7);
if(DMA1_Channel2->CNDTR == 0) {
DMA1_Channel2->CCR &= ~DMA_CCR_EN;
DMA1_Channel2->CNDTR = strlen(uart_buf);
DMA1_Channel2->CMAR = (uint32_t)uart_buf;
DMA1_Channel2->CCR |= DMA_CCR_EN;
}
#endif
}
void consoleLog(char *message)
{
HAL_UART_Transmit_DMA(&huart2, (uint8_t *)message, strlen(message));
}

235
Src/main.c
View File

@@ -21,6 +21,7 @@
#include "stm32f1xx_hal.h"
#include "defines.h"
#include "setup.h"
#include "config.h"
void SystemClock_Config(void);
@@ -30,231 +31,9 @@ extern ADC_HandleTypeDef hadc1;
extern ADC_HandleTypeDef hadc2;
extern volatile adc_buf_t adc_buffer;
volatile int posl = 0;
volatile int posr = 0;
volatile int pwml = 0;
volatile int pwmr = 0;
extern volatile int pwml;
extern volatile int pwmr;
const int pwm_res = 64000000 / 2 / PWM_FREQ;
const uint8_t hall_to_pos[8] = {
0,
0,
2,
1,
4,
5,
3,
0,
};
inline void block(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 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);
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.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.dcr - offsetdcr > 40) {
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;
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){
// timer = max_time;
// vel = 0;
// }
// if(pos != last_pos){
// vel = 1000 * PWM_FREQ / timer / P / 6 * 2;
// if((pos - last_pos + 6) % 6 > 2){
// vel = -vel;
// }
// timer = 0;
// }
// 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, 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);
}
int milli_vel_error_sum = 0;
@@ -323,7 +102,7 @@ void SystemClock_Config(void) {
RCC_ClkInitTypeDef RCC_ClkInitStruct;
RCC_PeriphCLKInitTypeDef PeriphClkInit;
/**Initializes the CPU, AHB and APB busses clocks
/**Initializes the CPU, AHB and APB busses clocks
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
@@ -333,7 +112,7 @@ void SystemClock_Config(void) {
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL16;
HAL_RCC_OscConfig(&RCC_OscInitStruct);
/**Initializes the CPU, AHB and APB busses clocks
/**Initializes the CPU, AHB and APB busses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
@@ -347,11 +126,11 @@ void SystemClock_Config(void) {
PeriphClkInit.AdcClockSelection = RCC_ADCPCLK2_DIV8;
HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit);
/**Configure the Systick interrupt time
/**Configure the Systick interrupt time
*/
HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq() / 1000);
/**Configure the Systick
/**Configure the Systick
*/
HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);

9
Src/setup.c
View File

@@ -35,6 +35,7 @@ pb10 usart3 dma1 channel2/3
*/
#include "defines.h"
#include "config.h"
TIM_HandleTypeDef htim_right;
TIM_HandleTypeDef htim_left;
@@ -48,7 +49,7 @@ void UART_Init() {
UART_HandleTypeDef huart3;
huart3.Instance = USART3;
huart3.Init.BaudRate = 115200;
huart3.Init.BaudRate = DEBUG_BAUD;
huart3.Init.WordLength = UART_WORDLENGTH_8B;
huart3.Init.StopBits = UART_STOPBITS_1;
huart3.Init.Parity = UART_PARITY_NONE;
@@ -330,11 +331,11 @@ void MX_ADC1_Init(void) {
hadc1.Init.NbrOfConversion = 5;
HAL_ADC_Init(&hadc1);
/**Enable or disable the remapping of ADC1_ETRGREG:
* ADC1 External Event regular conversion is connected to TIM8 TRG0
* ADC1 External Event regular conversion is connected to TIM8 TRG0
*/
__HAL_AFIO_REMAP_ADC1_ETRGREG_ENABLE();
/**Configure the ADC multi-mode
/**Configure the ADC multi-mode
*/
multimode.Mode = ADC_DUALMODE_REGSIMULT;
HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode);
@@ -388,7 +389,7 @@ void MX_ADC2_Init(void) {
// HAL_ADC_DeInit(&hadc2);
// hadc2.Instance->CR2 = 0;
/**Common config
/**Common config
*/
hadc2.Instance = ADC2;
hadc2.Init.ScanConvMode = ADC_SCAN_ENABLE;