feedc0de/bobbycar-foc-model
1
0
Fork 0
forked from EFeru/hoverboard-firmware-hack-FOC
Code Pull Requests Activity

Removed malloc and added more variables (debug protocol). Parameter for alternate adc wiring. Removed unnecessary limiting of cmdL and cmdR. Board_variant parameter for alternate boards

Browse Source
This commit is contained in:
Candas1
2021-02-20 16:53:22 +01:00
parent 508fdc1d5f
commit 7f4b922808
6 changed files with 125 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
Inc/config.h
View File

@ -58,7 +58,12 @@
#define ADC_TOTAL_CONV_TIME (ADC_CLOCK_DIV * ADC_CONV_CLOCK_CYCLES) // = ((SystemCoreClock / ADC_CLOCK_HZ) * ADC_CONV_CLOCK_CYCLES), where ADC_CLOCK_HZ = SystemCoreClock/ADC_CLOCK_DIV
// ########################### END OF DO-NOT-TOUCH SETTINGS ############################
// ############################### BOARD VARIANT ###############################
/* Board Variant
* 0 - Default board type
* 1 - Alternate board type with different pin mapping for DCLINK, Buzzer and ON/OFF, Button and Charger
*/
#define BOARD_VARIANT 0 // change if board with alternate pin mapping
// ############################### BATTERY ###############################
/* Battery voltage calibration: connect power source.
@ -293,6 +298,7 @@
#define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
#endif
// #define ADC_CONNECTION_ALTERNATE // use to swap ADC inputs
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2!
// #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
#endif
@ -499,6 +505,7 @@
#define SPEED_COEFFICIENT 16384 // 1.0f
#define STEER_COEFFICIENT 8192 // 0.5f Only active in Sideboard input
// #define ADC_CONNECTION_ALTERNATE // use to swap ADC inputs
// #define INVERT_R_DIRECTION // Invert rotation of right motor
// #define INVERT_L_DIRECTION // Invert rotation of left motor
// #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used!

32
Inc/defines.h
View File

@ -102,28 +102,56 @@
// #define DCLINK_ADC ADC3
// #define DCLINK_CHANNEL
#if BOARD_VARIANT == 0
#define DCLINK_PIN GPIO_PIN_2
#define DCLINK_PORT GPIOC
#elif BOARD_VARIANT == 1
#define DCLINK_PIN GPIO_PIN_1
#define DCLINK_PORT GPIOA
#endif
// #define DCLINK_PULLUP 30000
// #define DCLINK_PULLDOWN 1000
#define LED_PIN GPIO_PIN_2
#define LED_PORT GPIOB
#if BOARD_VARIANT == 0
#define BUZZER_PIN GPIO_PIN_4
#define BUZZER_PORT GPIOA
#elif BOARD_VARIANT == 1
#define BUZZER_PIN GPIO_PIN_13
#define BUZZER_PORT GPIOC
#endif
#define SWITCH_PIN GPIO_PIN_1
#define SWITCH_PORT GPIOA
// UNUSED/REDUNDANT
//#define SWITCH_PIN GPIO_PIN_1
//#define SWITCH_PORT GPIOA
#if BOARD_VARIANT == 0
#define OFF_PIN GPIO_PIN_5
#define OFF_PORT GPIOA
#elif BOARD_VARIANT == 1
#define OFF_PIN GPIO_PIN_15
#define OFF_PORT GPIOC
#endif
#if BOARD_VARIANT == 0
#define BUTTON_PIN GPIO_PIN_1
#define BUTTON_PORT GPIOA
#elif BOARD_VARIANT == 1
#define BUTTON_PIN GPIO_PIN_9
#define BUTTON_PORT GPIOB
#endif
#if BOARD_VARIANT == 0
#define CHARGER_PIN GPIO_PIN_12
#define CHARGER_PORT GPIOA
#elif BOARD_VARIANT == 1
#define CHARGER_PIN GPIO_PIN_11
#define CHARGER_PORT GPIOA
#endif
#if defined(CONTROL_PPM_LEFT)
#define PPM_PIN GPIO_PIN_3

24
Src/comms.c
View File

@ -56,11 +56,17 @@ extern InputStruct input1[]; // input structure
extern InputStruct input2[]; // input structure
extern uint16_t VirtAddVarTab[NB_OF_VAR];
extern int16_t speedAvg; // average measured speed
extern int16_t speedAvgAbs; // average measured speed in absolute
extern uint8_t ctrlModReqRaw;
extern adc_buf_t adc_buffer;
extern int16_t speedAvg; // average measured speed
extern int16_t speedAvgAbs; // average measured speed in absolute
extern uint8_t ctrlModReqRaw;
extern int16_t batVoltageCalib;
extern int16_t board_temp_deg_c;
extern int16_t left_dc_curr;
extern int16_t right_dc_curr;
extern int16_t dc_curr;
extern int16_t cmdL;
extern int16_t cmdR;
enum commandTypes {READ,WRITE};
@ -123,15 +129,19 @@ const parameter_entry params[] = {
#endif
// FEEDBACK
// Type ,Name ,Datatype, ValueL ptr ,ValueR ,EEPRM Addr ,Init Int/Ext ,Min ,Max ,Div ,Mul ,Fix ,Callback Function ,Help text
{VARIABLE ,"I_DC_LINK" ,ADD_PARAM(rtU_Left.i_DCLink) ,&rtU_Right.i_DCLink ,0 ,0 ,0 ,0 ,0 ,A2BIT_CONV ,100 ,0 ,NULL ,"DC Link current A *100"},
{VARIABLE ,"DC_CURR" ,ADD_PARAM(dc_curr) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Total DC Link current A *100"},
{VARIABLE ,"RDC_CURR" ,ADD_PARAM(right_dc_curr) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Right DC Link current A *100"},
{VARIABLE ,"LDC_CURR" ,ADD_PARAM(left_dc_curr) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Left DC Link current A *100"},
{VARIABLE ,"CMDL" ,ADD_PARAM(cmdL) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Left Motor Command"},
{VARIABLE ,"CMDR" ,ADD_PARAM(cmdR) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Right Motor Command"},
{VARIABLE ,"SPD_AVG" ,ADD_PARAM(speedAvg) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Motor Measured Avg RPM"},
{VARIABLE ,"SPDL" ,ADD_PARAM(rtY_Left.n_mot) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Left Motor Measured RPM"},
{VARIABLE ,"SPDR" ,ADD_PARAM(rtY_Right.n_mot) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Right Motor Measured RPM"},
{VARIABLE ,"RATE" ,0 , NULL ,NULL ,0 ,RATE ,0 ,0 ,0 ,0 ,0 ,4 ,NULL ,"Rate *10"},
{VARIABLE ,"SPD_COEF" ,0 , NULL ,NULL ,0 ,SPEED_COEFFICIENT ,0 ,0 ,0 ,0 ,10 ,14 ,NULL ,"Speed Coefficient *10"},
{VARIABLE ,"STR_COEF" ,0 , NULL ,NULL ,0 ,STEER_COEFFICIENT ,0 ,0 ,0 ,0 ,10 ,14 ,NULL ,"Steer Coefficient *10"},
{VARIABLE ,"BATV" ,ADD_PARAM(adc_buffer.batt1) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Battery voltage *100"},
//{VARIABLE ,"TEMP" ,ADD_PARAM(board_temp_deg_c) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Temperature °C *10"},
{VARIABLE ,"BATV" ,ADD_PARAM(batVoltageCalib) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Calibrated Battery voltage *100"},
{VARIABLE ,"TEMP" ,ADD_PARAM(board_temp_deg_c) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Calibrated Temperature °C *10"},
};

97
Src/main.c
View File

@ -61,6 +61,8 @@ extern P rtP_Left; /* Block parameters (auto storage) */
extern P rtP_Right; /* Block parameters (auto storage) */
extern ExtY rtY_Left; /* External outputs */
extern ExtY rtY_Right; /* External outputs */
extern ExtU rtU_Left; /* External inputs */
extern ExtU rtU_Right; /* External inputs */
//---------------
extern uint8_t inIdx; // input index used for dual-inputs
@ -102,6 +104,13 @@ extern volatile uint16_t pwm_captured_ch2_value;
//------------------------------------------------------------------------
uint8_t backwardDrive;
volatile uint32_t main_loop_counter;
int16_t batVoltageCalib; // global variable for calibrated battery voltage
int16_t board_temp_deg_c; // global variable for calibrated temperature in degrees Celsius
int16_t left_dc_curr; // global variable for Left DC Link current
int16_t right_dc_curr; // global variable for Right DC Link current
int16_t dc_curr; // global variable for Total DC Link current
int16_t cmdL; // global variable for Left Command
int16_t cmdR; // global variable for Right Command
//------------------------------------------------------------------------
// Local variables
@ -151,7 +160,6 @@ static int16_t speed; // local variable for speed. -1000 to 10
static uint32_t inactivity_timeout_counter;
static MultipleTap MultipleTapBrake; // define multiple tap functionality for the Brake pedal
int main(void) {
HAL_Init();
@ -191,13 +199,9 @@ int main(void) {
poweronMelody();
HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET);
int16_t cmdL = 0, cmdR = 0;
int16_t cmdL_prev = 0, cmdR_prev = 0;
int32_t board_temp_adcFixdt = adc_buffer.temp << 16; // Fixed-point filter output initialized with current ADC converted to fixed-point
int16_t board_temp_adcFilt = adc_buffer.temp;
int16_t board_temp_deg_c;
// Loop until button is released
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }
@ -293,18 +297,16 @@ int main(void) {
mixerFcn(speed << 4, steer << 4, &cmdR, &cmdL); // This function implements the equations above
// ####### SET OUTPUTS (if the target change is less than +/- 100) #######
if ((cmdL > cmdL_prev-100 && cmdL < cmdL_prev+100) && (cmdR > cmdR_prev-100 && cmdR < cmdR_prev+100)) {
#ifdef INVERT_R_DIRECTION
pwmr = cmdR;
#else
pwmr = -cmdR;
#endif
#ifdef INVERT_L_DIRECTION
pwml = -cmdL;
#else
pwml = cmdL;
#endif
}
#ifdef INVERT_R_DIRECTION
pwmr = cmdR;
#else
pwmr = -cmdR;
#endif
#ifdef INVERT_L_DIRECTION
pwml = -cmdL;
#else
pwml = cmdL;
#endif
#endif
#ifdef VARIANT_TRANSPOTTER
@ -315,32 +317,30 @@ int main(void) {
if (nunchuk_connected == 0) {
cmdL = cmdL * 0.8f + (CLAMP(distanceErr + (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f);
cmdR = cmdR * 0.8f + (CLAMP(distanceErr - (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f);
if ((cmdL < cmdL_prev + 50 && cmdL > cmdL_prev - 50) && (cmdR < cmdR_prev + 50 && cmdR > cmdR_prev - 50)) {
if (distanceErr > 0) {
enable = 1;
}
if (distanceErr > -300) {
#ifdef INVERT_R_DIRECTION
pwmr = cmdR;
#else
pwmr = -cmdR;
#endif
#ifdef INVERT_L_DIRECTION
pwml = -cmdL;
#else
pwml = cmdL;
#endif
if (distanceErr > 0) {
enable = 1;
}
if (distanceErr > -300) {
#ifdef INVERT_R_DIRECTION
pwmr = cmdR;
#else
pwmr = -cmdR;
#endif
#ifdef INVERT_L_DIRECTION
pwml = -cmdL;
#else
pwml = cmdL;
#endif
if (checkRemote) {
if (!HAL_GPIO_ReadPin(LED_PORT, LED_PIN)) {
//enable = 1;
} else {
enable = 0;
}
if (checkRemote) {
if (!HAL_GPIO_ReadPin(LED_PORT, LED_PIN)) {
//enable = 1;
} else {
enable = 0;
}
} else {
enable = 0;
}
} else {
enable = 0;
}
timeoutCntGen = 0;
timeoutFlgGen = 0;
@ -421,6 +421,14 @@ int main(void) {
board_temp_adcFilt = (int16_t)(board_temp_adcFixdt >> 16); // convert fixed-point to integer
board_temp_deg_c = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C;
// ####### CALC CALIBRATED BATTERY VOLTAGE #######
batVoltageCalib = batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC;
// ####### CALC DC LINK CURRENT #######
left_dc_curr = -(rtU_Left.i_DCLink * 100) / A2BIT_CONV; // Left DC Link Current * 100
right_dc_curr = -(rtU_Right.i_DCLink * 100) / A2BIT_CONV; // Right DC Link Current * 100
dc_curr = left_dc_curr + right_dc_curr; // Total DC Link Current * 100
// ####### DEBUG SERIAL OUT #######
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
if (main_loop_counter % 25 == 0) { // Send data periodically every 125 ms
@ -433,7 +441,7 @@ int main(void) {
cmdL, // 3: output command: [-1000, 1000]
cmdR, // 4: output command: [-1000, 1000]
adc_buffer.batt1, // 5: for battery voltage calibration
batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC, // 6: for verifying battery voltage calibration
batVoltageCalib, // 6: for verifying battery voltage calibration
board_temp_adcFilt, // 7: for board temperature calibration
board_temp_deg_c); // 8: for verifying board temperature calibration
#endif
@ -448,7 +456,7 @@ int main(void) {
Feedback.cmd2 = (int16_t)input2[inIdx].cmd;
Feedback.speedR_meas = (int16_t)rtY_Right.n_mot;
Feedback.speedL_meas = (int16_t)rtY_Left.n_mot;
Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC);
Feedback.batVoltage = (int16_t)batVoltageCalib;
Feedback.boardTemp = (int16_t)board_temp_deg_c;
#if defined(FEEDBACK_SERIAL_USART2)
@ -512,11 +520,10 @@ int main(void) {
poweroff();
}
// HAL_GPIO_TogglePin(LED_PORT, LED_PIN); // This is to measure the main() loop duration with an oscilloscope connected to LED_PIN
// Update states
inIdx_prev = inIdx;
cmdL_prev = cmdL;
cmdR_prev = cmdR;
main_loop_counter++;
}
}

4
Src/setup.c
View File

@ -638,7 +638,11 @@ void MX_ADC1_Init(void) {
sConfig.Rank = 3;
HAL_ADC_ConfigChannel(&hadc1, &sConfig);
#if BOARD_VARIANT == 0
sConfig.Channel = ADC_CHANNEL_12; // pc2 vbat
#elif BOARD_VARIANT == 1
sConfig.Channel = ADC_CHANNEL_1; // pa1 vbat
#endif
sConfig.Rank = 4;
HAL_ADC_ConfigChannel(&hadc1, &sConfig);

30
Src/util.c
View File

@ -802,8 +802,13 @@ void calcInputCmd(InputStruct *in, int16_t out_min, int16_t out_max) {
void readInputRaw(void) {
#ifdef CONTROL_ADC
if (inIdx == CONTROL_ADC) {
input1[inIdx].raw = adc_buffer.l_tx2;
input2[inIdx].raw = adc_buffer.l_rx2;
#ifndef ADC_CONNECTION_ALTERNATE
input1[inIdx].raw = adc_buffer.l_tx2;
input2[inIdx].raw = adc_buffer.l_rx2;
#else
input1[inIdx].raw = adc_buffer.l_rx2;
input2[inIdx].raw = adc_buffer.l_tx2;
#endif
}
#endif
@ -1065,20 +1070,16 @@ void usart2_rx_check(void)
#endif
#if defined(DEBUG_SERIAL_USART2)
uint8_t *ptr;
uint8_t ptr[SERIAL_BUFFER_SIZE];
if (pos != old_pos) { // Check change in received data
if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one
usart_process_debug(&rx_buffer_L[old_pos], pos - old_pos); // Process data
} else { // "Overflow" buffer mode
ptr = (uint8_t *) malloc(sizeof(uint8_t) * (rx_buffer_L_len - old_pos + pos));
memcpy(ptr, &rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First copy data from the end of buffer
memcpy(&ptr[0], &rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First copy data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
ptr += rx_buffer_L_len - old_pos;
memcpy(ptr, &rx_buffer_L[0], pos); // Copy remaining data
memcpy(&ptr[rx_buffer_L_len - old_pos], &rx_buffer_L[0], pos); // Copy remaining data
}
ptr -= rx_buffer_L_len - old_pos;
usart_process_debug(ptr, rx_buffer_L_len - old_pos + pos); // Process data
free(ptr);
}
}
#endif // DEBUG_SERIAL_USART2
@ -1141,20 +1142,17 @@ void usart3_rx_check(void)
#endif
#if defined(DEBUG_SERIAL_USART3)
uint8_t *ptr;
uint8_t ptr[SERIAL_BUFFER_SIZE];
if (pos != old_pos) { // Check change in received data
if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one
usart_process_debug(&rx_buffer_R[old_pos], pos - old_pos); // Process data
} else { // "Overflow" buffer mode
ptr = (uint8_t *) malloc(sizeof(uint8_t) * (rx_buffer_R_len - old_pos + pos));
memcpy(ptr, &rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First copy data from the end of buffer
memcpy(&ptr[0], &rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First copy data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
ptr += rx_buffer_R_len - old_pos;
memcpy(ptr, &rx_buffer_R[0], pos); // Copy remaining data
memcpy(&ptr[rx_buffer_R_len - old_pos], &rx_buffer_R[0], pos); // Copy remaining data
}
ptr -= rx_buffer_R_len - old_pos;
usart_process_debug(ptr, rx_buffer_R_len - old_pos + pos); // Process data
free(ptr);
}
}
#endif // DEBUG_SERIAL_USART3