forked from lucysrausch/hoverboard-firmware-hack
Added new BLDC motor control
This commit is contained in:
EmanuelFeru
284
Src/bldc.c
284
Src/bldc.c
@@ -1,16 +1,38 @@
|
||||
/*
|
||||
* This file has been re-implemented with 4 new selectable motor control methods.
|
||||
* Recommended control method: 3 = Sinusoidal 3rd order. This control method offers superior performanace
|
||||
* compared to previous method. The new method features:
|
||||
* ► reduced noise and vibrations
|
||||
* ► smooth torque output
|
||||
* ► improved motor efficiency -> lower energy consumption
|
||||
*
|
||||
* Copyright (C) 2019 Emanuel FERU <aerdronix@gmail.com>
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "stm32f1xx_hal.h"
|
||||
#include "defines.h"
|
||||
#include "setup.h"
|
||||
#include "config.h"
|
||||
|
||||
// Matlab includes
|
||||
#include "BLDC_controller.h" /* Model's header file */
|
||||
#include "rtwtypes.h"
|
||||
|
||||
volatile int posl = 0;
|
||||
volatile int posr = 0;
|
||||
volatile int pwml = 0;
|
||||
volatile int pwmr = 0;
|
||||
volatile int weakl = 0;
|
||||
volatile int weakr = 0;
|
||||
|
||||
extern volatile int speed;
|
||||
|
||||
@@ -18,110 +40,13 @@ extern volatile adc_buf_t adc_buffer;
|
||||
|
||||
extern volatile uint32_t timeout;
|
||||
|
||||
uint32_t buzzerFreq = 0;
|
||||
uint32_t buzzerPattern = 0;
|
||||
uint32_t buzzerFreq = 0;
|
||||
uint32_t buzzerPattern = 0;
|
||||
uint32_t buzzerTimer = 0;
|
||||
|
||||
uint8_t enable = 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;
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t buzzerTimer = 0;
|
||||
const int pwm_res = 64000000 / 2 / PWM_FREQ; // = 2000
|
||||
|
||||
int offsetcount = 0;
|
||||
int offsetrl1 = 2000;
|
||||
@@ -133,20 +58,11 @@ int offsetdcr = 2000;
|
||||
|
||||
float batteryVoltage = BAT_NUMBER_OF_CELLS * 4.0;
|
||||
|
||||
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;
|
||||
|
||||
//scan 8 channels with 2ADCs @ 20 clk cycles per sample
|
||||
//meaning ~80 ADC clock cycles @ 8MHz until new DMA interrupt =~ 100KHz
|
||||
//=640 cpu cycles
|
||||
void DMA1_Channel1_IRQHandler() {
|
||||
|
||||
DMA1->IFCR = DMA_IFCR_CTCIF1;
|
||||
// HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
|
||||
|
||||
@@ -180,61 +96,6 @@ void DMA1_Channel1_IRQHandler() {
|
||||
RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
|
||||
}
|
||||
|
||||
int ul, vl, wl;
|
||||
int ur, vr, wr;
|
||||
|
||||
//determine next position based on hall sensors
|
||||
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);
|
||||
|
||||
|
||||
// uint8_t buzz(uint16_t *notes, uint32_t len){
|
||||
// static uint32_t counter = 0;
|
||||
// static uint32_t timer = 0;
|
||||
// if(len == 0){
|
||||
// return(0);
|
||||
// }
|
||||
|
||||
// struct {
|
||||
// uint16_t freq : 4;
|
||||
// uint16_t volume : 4;
|
||||
// uint16_t time : 8;
|
||||
// } note = notes[counter];
|
||||
|
||||
// if(timer / 500 == note.time){
|
||||
// timer = 0;
|
||||
// counter++;
|
||||
// }
|
||||
|
||||
// if(counter == len){
|
||||
// counter = 0;
|
||||
// }
|
||||
|
||||
// timer++;
|
||||
// return(note.freq);
|
||||
// }
|
||||
|
||||
|
||||
//create square wave for buzzer
|
||||
buzzerTimer++;
|
||||
if (buzzerFreq != 0 && (buzzerTimer / 5000) % (buzzerPattern + 1) == 0) {
|
||||
@@ -245,35 +106,64 @@ void DMA1_Channel1_IRQHandler() {
|
||||
HAL_GPIO_WritePin(BUZZER_PORT, BUZZER_PIN, 0);
|
||||
}
|
||||
|
||||
//update PWM channels based on position
|
||||
blockPWM(pwml, posl, &ul, &vl, &wl);
|
||||
blockPWM(pwmr, posr, &ur, &vr, &wr);
|
||||
// ############################### MOTOR CONTROL ###############################
|
||||
int ul, vl, wl;
|
||||
int ur, vr, wr;
|
||||
|
||||
int weakul, weakvl, weakwl;
|
||||
if (pwml > 0) {
|
||||
blockPWM(weakl, (posl+5) % 6, &weakul, &weakvl, &weakwl);
|
||||
} else {
|
||||
blockPWM(-weakl, (posl+1) % 6, &weakul, &weakvl, &weakwl);
|
||||
// Get hall sensors values
|
||||
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);
|
||||
|
||||
static boolean_T OverrunFlag = false;
|
||||
/* Check for overrun */
|
||||
if (OverrunFlag) {
|
||||
return;
|
||||
}
|
||||
ul += weakul;
|
||||
vl += weakvl;
|
||||
wl += weakwl;
|
||||
OverrunFlag = true;
|
||||
|
||||
/* Set motor inputs here */
|
||||
rtU.b_hallALeft = hall_ul;
|
||||
rtU.b_hallBLeft = hall_vl;
|
||||
rtU.b_hallCLeft = hall_wl;
|
||||
rtU.r_DCLeft = pwml;
|
||||
|
||||
int weakur, weakvr, weakwr;
|
||||
if (pwmr > 0) {
|
||||
blockPWM(weakr, (posr+5) % 6, &weakur, &weakvr, &weakwr);
|
||||
} else {
|
||||
blockPWM(-weakr, (posr+1) % 6, &weakur, &weakvr, &weakwr);
|
||||
}
|
||||
ur += weakur;
|
||||
vr += weakvr;
|
||||
wr += weakwr;
|
||||
rtU.b_hallARight = hall_ur;
|
||||
rtU.b_hallBRight = hall_vr;
|
||||
rtU.b_hallCRight = hall_wr;
|
||||
rtU.r_DCRight = pwmr;
|
||||
|
||||
/* Step the controller */
|
||||
BLDC_controller_step();
|
||||
|
||||
/* Get motor outputs here */
|
||||
ul = rtY.DC_phaALeft;
|
||||
vl = rtY.DC_phaBLeft;
|
||||
wl = rtY.DC_phaCLeft;
|
||||
// motSpeedLeft = rtY.n_motLeft;
|
||||
// motAngleLeft = rtY.a_elecAngleLeft;
|
||||
|
||||
ur = rtY.DC_phaARight;
|
||||
vr = rtY.DC_phaBRight;
|
||||
wr = rtY.DC_phaCRight;
|
||||
// motSpeedRight = rtY.n_motRight;
|
||||
// motAngleRight = rtY.a_elecAngleRight;
|
||||
|
||||
/* Indicate task complete */
|
||||
OverrunFlag = false;
|
||||
// ###############################################################################
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user