interfisch
/
hoverboard-firmware-hack-serial-esc
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

current feedback "working"

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Inc/defines.h
View File

@ -118,7 +118,7 @@
#define CHARGER_PORT GPIOA #define CHARGER_PORT GPIOA
#define PWM_FREQ 16000 #define PWM_FREQ 16000
#define DEAD_TIME 50 #define DEAD_TIME 32
#define R 0.27 #define R 0.27
#define P 15 #define P 15
@ -151,8 +151,8 @@ typedef struct {
uint16_t rr2; uint16_t rr2;
uint16_t rl1; uint16_t rl1;
uint16_t rl2; uint16_t rl2;
uint16_t r_dc1; uint16_t dcr;
uint16_t l_dc2; uint16_t dcl;
uint16_t batt1; uint16_t batt1;
uint16_t l_tx2; uint16_t l_tx2;
uint16_t bat1; uint16_t bat1;

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 last_pos = 0;
int timer = 0; int timer = 0;
int max_time = PWM_FREQ / 10; int max_time = PWM_FREQ / 10;
volatile int vel = 0; 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]; volatile uint8_t uart_buf[10];
int curl = 0;
// int errorl = 0;
// int kp = 5;
// volatile int cmdl = 0;
void DMA1_Channel1_IRQHandler() { void DMA1_Channel1_IRQHandler() {
DMA1->IFCR = DMA_IFCR_CTCIF1; DMA1->IFCR = DMA_IFCR_CTCIF1;
HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); // 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';
if(DMA1_Channel2->CNDTR == 0){ if(offsetcount < 1000) {
DMA1_Channel2->CCR &= ~DMA_CCR_EN; offsetcount++;
DMA1_Channel2->CNDTR = 10; offsetrl1 = (adc_buffer.rl1 + offsetrl1) / 2;
DMA1_Channel2->CMAR = (uint32_t)uart_buf; offsetrl2 = (adc_buffer.rl2 + offsetrl2) / 2;
DMA1_Channel2->CCR |= DMA_CCR_EN; 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; LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
} else { } else {
LEFT_TIM->BDTR |= TIM_BDTR_MOE; 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; RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;
} else { } else {
RIGHT_TIM->BDTR |= TIM_BDTR_MOE; RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
} }
int ul = 0; int ul, vl, wl;
int vl = 0; int ur, vr, wr;
int wl = 0;
int ur = 0;
int vr = 0;
int wr = 0;
uint8_t hall_ul = !(LEFT_HALL_U_PORT->IDR & LEFT_HALL_U_PIN); 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_vl = !(LEFT_HALL_V_PORT->IDR & LEFT_HALL_V_PIN);
@ -155,6 +202,25 @@ void DMA1_Channel1_IRQHandler() {
posr += 2; posr += 2;
posr %= 6; 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++; timer++;
// if(timer > max_time){ // if(timer > max_time){
@ -173,17 +239,21 @@ void DMA1_Channel1_IRQHandler() {
// } // }
// last_pos = pos; // last_pos = pos;
//YOLOTEST
// errorl = cmdl - curl;
// pwml = kp * errorl;
block(pwml, posl, &ul, &vl, &wl); block(pwml, posl, &ul, &vl, &wl);
block(pwmr, posr, &ur, &vr, &wr); block(pwmr, posr, &ur, &vr, &wr);
LEFT_TIM->LEFT_TIM_U = CLAMP(ul + 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, 0, pwm_res); LEFT_TIM->LEFT_TIM_V = CLAMP(vl + pwm_res / 2, 10, pwm_res-10);
LEFT_TIM->LEFT_TIM_W = CLAMP(wl + pwm_res / 2, 0, pwm_res); 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_U = CLAMP(ur + pwm_res / 2, 10, pwm_res-10);
RIGHT_TIM->RIGHT_TIM_V = CLAMP(vr + pwm_res / 2, 0, pwm_res); RIGHT_TIM->RIGHT_TIM_V = CLAMP(vr + pwm_res / 2, 10, pwm_res-10);
RIGHT_TIM->RIGHT_TIM_W = CLAMP(wr + pwm_res / 2, 0, pwm_res); RIGHT_TIM->RIGHT_TIM_W = CLAMP(wr + pwm_res / 2, 10, pwm_res-10);
HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0); // HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0);
} }
int milli_vel_error_sum = 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 += milli_vel_error;
// milli_vel_error_sum = CLAMP(milli_vel_error_sum, -200000, 200000); // milli_vel_error_sum = CLAMP(milli_vel_error_sum, -200000, 200000);
// pwm = CLAMP(milli_vel_cmd / 5 + milli_vel_error_sum / 200, -500, 500); // pwm = CLAMP(milli_vel_cmd / 5 + milli_vel_error_sum / 200, -500, 500);
// cmdl = 70;
pwml = 150; pwml = 150;
pwmr = 150; pwmr = 150;

45
Src/setup.c
View File

@ -73,6 +73,38 @@ void UART_Init() {
DMA1->IFCR = DMA_IFCR_CTCIF2 | DMA_IFCR_CHTIF2 | DMA_IFCR_CGIF2; DMA1->IFCR = DMA_IFCR_CTCIF2 | DMA_IFCR_CHTIF2 | DMA_IFCR_CGIF2;
} }
/*
void UART_Init() {
__HAL_RCC_USART2_CLK_ENABLE();
__HAL_RCC_DMA1_CLK_ENABLE();
UART_HandleTypeDef huart2;
huart2.Instance = USART2;
huart2.Init.BaudRate = 115200;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
HAL_UART_Init(&huart2);
USART2->CR3 |= USART_CR3_DMAT; // | USART_CR3_DMAR | USART_CR3_OVRDIS;
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = GPIO_PIN_2;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
DMA1_Channel7->CCR = 0;
DMA1_Channel7->CPAR = (uint32_t) & (USART3->DR);
DMA1_Channel7->CNDTR = 0;
DMA1_Channel7->CCR = DMA_CCR_MINC | DMA_CCR_DIR;
DMA1->IFCR = DMA_IFCR_CTCIF7 | DMA_IFCR_CHTIF7 | DMA_IFCR_CGIF7;
}
*/
void MX_GPIO_Init(void) { void MX_GPIO_Init(void) {
GPIO_InitTypeDef GPIO_InitStruct; GPIO_InitTypeDef GPIO_InitStruct;
@ -261,6 +293,9 @@ void MX_TIM_Init(void) {
sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
HAL_TIMEx_ConfigBreakDeadTime(&htim_left, &sBreakDeadTimeConfig); HAL_TIMEx_ConfigBreakDeadTime(&htim_left, &sBreakDeadTimeConfig);
LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;
HAL_TIM_PWM_Start(&htim_left, TIM_CHANNEL_1); HAL_TIM_PWM_Start(&htim_left, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim_left, TIM_CHANNEL_2); HAL_TIM_PWM_Start(&htim_left, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim_left, TIM_CHANNEL_3); HAL_TIM_PWM_Start(&htim_left, TIM_CHANNEL_3);
@ -304,16 +339,18 @@ void MX_ADC1_Init(void) {
multimode.Mode = ADC_DUALMODE_REGSIMULT; multimode.Mode = ADC_DUALMODE_REGSIMULT;
HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode); HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode);
sConfig.SamplingTime = ADC_SAMPLETIME_13CYCLES_5; sConfig.SamplingTime = ADC_SAMPLETIME_7CYCLES_5;
sConfig.Channel = ADC_CHANNEL_14; sConfig.Channel = ADC_CHANNEL_14;
sConfig.Rank = 1; sConfig.Rank = 1;
HAL_ADC_ConfigChannel(&hadc1, &sConfig); HAL_ADC_ConfigChannel(&hadc1, &sConfig);
sConfig.Channel = ADC_CHANNEL_1; sConfig.Channel = ADC_CHANNEL_0;
sConfig.Rank = 2; sConfig.Rank = 2;
HAL_ADC_ConfigChannel(&hadc1, &sConfig); HAL_ADC_ConfigChannel(&hadc1, &sConfig);
sConfig.SamplingTime = ADC_SAMPLETIME_13CYCLES_5;
sConfig.Channel = ADC_CHANNEL_11; sConfig.Channel = ADC_CHANNEL_11;
sConfig.Rank = 3; sConfig.Rank = 3;
HAL_ADC_ConfigChannel(&hadc1, &sConfig); HAL_ADC_ConfigChannel(&hadc1, &sConfig);
@ -362,7 +399,7 @@ void MX_ADC2_Init(void) {
hadc2.Init.NbrOfConversion = 5; hadc2.Init.NbrOfConversion = 5;
HAL_ADC_Init(&hadc2); HAL_ADC_Init(&hadc2);
sConfig.SamplingTime = ADC_SAMPLETIME_13CYCLES_5; sConfig.SamplingTime = ADC_SAMPLETIME_7CYCLES_5;
sConfig.Channel = ADC_CHANNEL_15; sConfig.Channel = ADC_CHANNEL_15;
sConfig.Rank = 1; sConfig.Rank = 1;
@ -372,6 +409,8 @@ void MX_ADC2_Init(void) {
sConfig.Rank = 2; sConfig.Rank = 2;
HAL_ADC_ConfigChannel(&hadc2, &sConfig); HAL_ADC_ConfigChannel(&hadc2, &sConfig);
sConfig.SamplingTime = ADC_SAMPLETIME_13CYCLES_5;
sConfig.Channel = ADC_CHANNEL_10; sConfig.Channel = ADC_CHANNEL_10;
sConfig.Rank = 3; sConfig.Rank = 3;
HAL_ADC_ConfigChannel(&hadc2, &sConfig); HAL_ADC_ConfigChannel(&hadc2, &sConfig);