forked from lucysrausch/hoverboard-firmware-hack
► Improved controller: faster and better overall
- the controller is faster and more light in memory footprint - motor control is now done intependenly using Reusable code generation setting in Simulink - fixed bug when chaging direction fast
This commit is contained in:
EmanuelFeru
File diff suppressed because it is too large
Load Diff
@@ -7,9 +7,9 @@
|
||||
*
|
||||
* Code generated for Simulink model 'BLDC_controller'.
|
||||
*
|
||||
* Model version : 1.817
|
||||
* Model version : 1.877
|
||||
* Simulink Coder version : 8.13 (R2017b) 24-Jul-2017
|
||||
* C/C++ source code generated on : Tue May 28 19:55:33 2019
|
||||
* C/C++ source code generated on : Wed Jun 5 22:29:28 2019
|
||||
*
|
||||
* Target selection: ert.tlc
|
||||
* Embedded hardware selection: ARM Compatible->ARM Cortex
|
||||
@@ -23,185 +23,133 @@
|
||||
|
||||
#include "BLDC_controller.h"
|
||||
|
||||
/* Block parameters (auto storage) */
|
||||
P rtP = {
|
||||
/* Variable: cf_speedCoef
|
||||
* Referenced by:
|
||||
* '<S28>/cf_spdCoef'
|
||||
* '<S66>/cf_spdCoef'
|
||||
*/
|
||||
66667,
|
||||
|
||||
/* Variable: n_commAcvLo
|
||||
* Referenced by:
|
||||
* '<S15>/Relay'
|
||||
* '<S53>/Relay'
|
||||
*/
|
||||
100,
|
||||
|
||||
/* Variable: n_commDeacvHi
|
||||
* Referenced by:
|
||||
* '<S15>/Relay'
|
||||
* '<S53>/Relay'
|
||||
*/
|
||||
180,
|
||||
|
||||
/* Variable: r_commDCDeacv
|
||||
* Referenced by:
|
||||
* '<S15>/r_commDCDeacv'
|
||||
* '<S53>/r_commDCDeacv'
|
||||
*/
|
||||
70,
|
||||
|
||||
/* Variable: r_phaAdvDC_XA
|
||||
* Referenced by:
|
||||
* '<S13>/r_phaAdvDC_XA'
|
||||
* '<S51>/r_phaAdvDC_XA'
|
||||
*/
|
||||
{ 0, 100, 200, 300, 400, 500, 600, 700, 800, 900, 1000 },
|
||||
|
||||
/* Variable: a_phaAdv_M1
|
||||
* Referenced by:
|
||||
* '<S13>/a_phaAdv_M2'
|
||||
* '<S51>/a_phaAdv_M2'
|
||||
*/
|
||||
{ 0, 0, 7, 2, 2, 2, 4, 5, 9, 16, 25 },
|
||||
|
||||
/* Variable: z_maxCntRst
|
||||
* Referenced by:
|
||||
* '<S23>/z_maxCntRst'
|
||||
* '<S23>/z_maxCntRst1'
|
||||
* '<S23>/z_maxCntRst2'
|
||||
* '<S23>/z_counter2'
|
||||
* '<S61>/z_maxCntRst'
|
||||
* '<S61>/z_maxCntRst1'
|
||||
* '<S61>/z_maxCntRst2'
|
||||
* '<S61>/z_counter2'
|
||||
* '<S28>/z_maxCntRst'
|
||||
* '<S66>/z_maxCntRst'
|
||||
*/
|
||||
2000,
|
||||
|
||||
/* Variable: z_ctrlTypSel
|
||||
* Referenced by:
|
||||
* '<S12>/z_ctrlTypSel1'
|
||||
* '<S50>/z_ctrlTypSel1'
|
||||
*/
|
||||
3U,
|
||||
|
||||
/* Variable: z_nrEdgeSpdAcv
|
||||
* Referenced by:
|
||||
* '<S23>/z_nrEdgeSpdAcv'
|
||||
* '<S61>/z_nrEdgeSpdAcv'
|
||||
*/
|
||||
3U,
|
||||
|
||||
/* Variable: b_phaAdvEna
|
||||
* Referenced by:
|
||||
* '<S13>/a_elecPeriod1'
|
||||
* '<S51>/a_elecPeriod1'
|
||||
*/
|
||||
1
|
||||
};
|
||||
|
||||
/* Constant parameters (auto storage) */
|
||||
const ConstP rtConstP = {
|
||||
/* Pooled Parameter (Expression: r_trapPhaA_M1)
|
||||
* Referenced by:
|
||||
* '<S41>/r_trapPhaA_M1'
|
||||
* '<S79>/r_trapPhaA_M1'
|
||||
/* Computed Parameter: r_trapPhaA_M1_Table
|
||||
* Referenced by: '<S18>/r_trapPhaA_M1'
|
||||
*/
|
||||
{ 1000, 1000, 1000, -1000, -1000, -1000, 1000 },
|
||||
|
||||
/* Pooled Parameter (Expression: r_trapPhaB_M1)
|
||||
* Referenced by:
|
||||
* '<S41>/r_trapPhaB_M1'
|
||||
* '<S79>/r_trapPhaB_M1'
|
||||
/* Computed Parameter: r_trapPhaB_M1_Table
|
||||
* Referenced by: '<S18>/r_trapPhaB_M1'
|
||||
*/
|
||||
{ -1000, -1000, 1000, 1000, 1000, -1000, -1000 },
|
||||
|
||||
/* Pooled Parameter (Expression: r_trapPhaC_M1)
|
||||
* Referenced by:
|
||||
* '<S41>/r_trapPhaC_M1'
|
||||
* '<S79>/r_trapPhaC_M1'
|
||||
/* Computed Parameter: r_trapPhaC_M1_Table
|
||||
* Referenced by: '<S18>/r_trapPhaC_M1'
|
||||
*/
|
||||
{ 1000, -1000, -1000, -1000, 1000, 1000, 1000 },
|
||||
|
||||
/* Pooled Parameter (Expression: r_sinPhaA_M1)
|
||||
* Referenced by:
|
||||
* '<S43>/r_sinPhaA_M1'
|
||||
* '<S81>/r_sinPhaA_M1'
|
||||
/* Computed Parameter: r_sinPhaA_M1_Table
|
||||
* Referenced by: '<S19>/r_sinPhaA_M1'
|
||||
*/
|
||||
{ 500, 643, 766, 866, 940, 985, 1000, 985, 940, 866, 766, 643, 500, 342, 174,
|
||||
0, -174, -342, -500, -643, -766, -866, -940, -985, -1000, -985, -940, -866,
|
||||
-766, -643, -500, -342, -174, 0, 174, 342, 500 },
|
||||
|
||||
/* Pooled Parameter (Expression: r_sinPhaB_M1)
|
||||
* Referenced by:
|
||||
* '<S43>/r_sinPhaB_M1'
|
||||
* '<S81>/r_sinPhaB_M1'
|
||||
/* Computed Parameter: r_sinPhaB_M1_Table
|
||||
* Referenced by: '<S19>/r_sinPhaB_M1'
|
||||
*/
|
||||
{ -1000, -985, -940, -866, -766, -643, -500, -342, -174, 0, 174, 342, 500, 643,
|
||||
766, 866, 940, 985, 1000, 985, 940, 866, 766, 643, 500, 342, 174, 0, -174,
|
||||
-342, -500, -643, -766, -866, -940, -985, -1000 },
|
||||
|
||||
/* Pooled Parameter (Expression: r_sinPhaC_M1)
|
||||
* Referenced by:
|
||||
* '<S43>/r_sinPhaC_M1'
|
||||
* '<S81>/r_sinPhaC_M1'
|
||||
/* Computed Parameter: r_sinPhaC_M1_Table
|
||||
* Referenced by: '<S19>/r_sinPhaC_M1'
|
||||
*/
|
||||
{ 500, 342, 174, 0, -174, -342, -500, -643, -766, -866, -940, -985, -1000,
|
||||
-985, -940, -866, -766, -643, -500, -342, -174, 0, 174, 342, 500, 643, 766,
|
||||
866, 940, 985, 1000, 985, 940, 866, 766, 643, 500 },
|
||||
|
||||
/* Pooled Parameter (Expression: r_sin3PhaA_M1)
|
||||
* Referenced by:
|
||||
* '<S42>/r_sin3PhaA_M1'
|
||||
* '<S80>/r_sin3PhaA_M1'
|
||||
/* Computed Parameter: r_sin3PhaA_M1_Table
|
||||
* Referenced by: '<S20>/r_sin3PhaA_M1'
|
||||
*/
|
||||
{ 795, 930, 991, 996, 971, 942, 930, 942, 971, 996, 991, 930, 795, 584, 310, 0,
|
||||
-310, -584, -795, -930, -991, -996, -971, -942, -930, -942, -971, -996, -991,
|
||||
-930, -795, -584, -310, 0, 310, 584, 795 },
|
||||
|
||||
/* Pooled Parameter (Expression: r_sin3PhaB_M1)
|
||||
* Referenced by:
|
||||
* '<S42>/r_sin3PhaB_M1'
|
||||
* '<S80>/r_sin3PhaB_M1'
|
||||
/* Computed Parameter: r_sin3PhaB_M1_Table
|
||||
* Referenced by: '<S20>/r_sin3PhaB_M1'
|
||||
*/
|
||||
{ -930, -942, -971, -996, -991, -930, -795, -584, -310, 0, 310, 584, 795, 930,
|
||||
991, 996, 971, 942, 930, 942, 971, 996, 991, 930, 795, 584, 310, 0, -310,
|
||||
-584, -795, -930, -991, -996, -971, -942, -930 },
|
||||
|
||||
/* Pooled Parameter (Expression: r_sin3PhaC_M1)
|
||||
* Referenced by:
|
||||
* '<S42>/r_sin3PhaC_M1'
|
||||
* '<S80>/r_sin3PhaC_M1'
|
||||
/* Computed Parameter: r_sin3PhaC_M1_Table
|
||||
* Referenced by: '<S20>/r_sin3PhaC_M1'
|
||||
*/
|
||||
{ 795, 584, 310, 0, -310, -584, -795, -930, -991, -996, -971, -942, -930, -942,
|
||||
-971, -996, -991, -930, -795, -584, -310, 0, 310, 584, 795, 930, 991, 996,
|
||||
971, 942, 930, 942, 971, 996, 991, 930, 795 },
|
||||
|
||||
/* Pooled Parameter (Expression: z_commutMap_M1)
|
||||
* Referenced by:
|
||||
* '<S15>/z_commutMap_M1'
|
||||
* '<S53>/z_commutMap_M1'
|
||||
/* Computed Parameter: z_commutMap_M1_table
|
||||
* Referenced by: '<S10>/z_commutMap_M1'
|
||||
*/
|
||||
{ 1000, -1000, 0, 1000, 0, -1000, 0, 1000, -1000, -1000, 1000, 0, -1000, 0,
|
||||
1000, 0, -1000, 1000 },
|
||||
|
||||
/* Pooled Parameter (Expression: vec_hallToPos)
|
||||
* Referenced by:
|
||||
* '<S21>/vec_hallToPos'
|
||||
* '<S59>/vec_hallToPos'
|
||||
/* Computed Parameter: vec_hallToPos_Value
|
||||
* Referenced by: '<S12>/vec_hallToPos'
|
||||
*/
|
||||
{ 0U, 5U, 3U, 4U, 1U, 0U, 2U, 0U },
|
||||
{ 0, 5, 3, 4, 1, 0, 2, 0 }
|
||||
};
|
||||
|
||||
/* Pooled Parameter (Expression: [0 1;1 0;0 1;0 1;1 0;1 0;0 0;0 0])
|
||||
* Referenced by:
|
||||
* '<S29>/Logic'
|
||||
* '<S67>/Logic'
|
||||
/* Tunable parameters (auto storage) */
|
||||
P rtP = {
|
||||
/* Variable: cf_speedCoef
|
||||
* Referenced by: '<S16>/cf_spdCoef'
|
||||
*/
|
||||
{ 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 0, 0, 0 }
|
||||
11111,
|
||||
|
||||
/* Variable: cf_speedFilt
|
||||
* Referenced by: '<S16>/cf_speedFilt'
|
||||
*/
|
||||
10,
|
||||
|
||||
/* Variable: n_commAcvLo
|
||||
* Referenced by: '<S14>/n_commDeacv'
|
||||
*/
|
||||
15,
|
||||
|
||||
/* Variable: n_commDeacvHi
|
||||
* Referenced by: '<S14>/n_commDeacv'
|
||||
*/
|
||||
30,
|
||||
|
||||
/* Variable: r_commDCDeacv
|
||||
* Referenced by: '<S14>/r_commDCDeacv'
|
||||
*/
|
||||
70,
|
||||
|
||||
/* Variable: r_phaAdvDC_XA
|
||||
* Referenced by: '<S8>/r_phaAdvDC_XA'
|
||||
*/
|
||||
{ 0, 100, 200, 300, 400, 500, 600, 700, 800, 900, 1000 },
|
||||
|
||||
/* Variable: a_phaAdv_M1
|
||||
* Referenced by: '<S8>/a_phaAdv_M2'
|
||||
*/
|
||||
{ 0, 0, 0, 0, 0, 2, 3, 5, 9, 16, 25 },
|
||||
|
||||
/* Variable: dz_counterHi
|
||||
* Referenced by: '<S14>/dz_counter'
|
||||
*/
|
||||
50,
|
||||
|
||||
/* Variable: dz_counterLo
|
||||
* Referenced by: '<S14>/dz_counter'
|
||||
*/
|
||||
20,
|
||||
|
||||
/* Variable: z_ctrlTypSel
|
||||
* Referenced by: '<S7>/z_ctrlTypSel1'
|
||||
*/
|
||||
3U,
|
||||
|
||||
/* Variable: b_phaAdvEna
|
||||
* Referenced by: '<S8>/a_elecPeriod1'
|
||||
*/
|
||||
1
|
||||
};
|
||||
|
||||
/*
|
||||
|
||||
116
Src/bldc.c
116
Src/bldc.c
@@ -27,15 +27,27 @@
|
||||
#include "setup.h"
|
||||
#include "config.h"
|
||||
|
||||
// Matlab includes
|
||||
// Matlab includes and defines - from auto-code generation
|
||||
// ###############################################################################
|
||||
#include "BLDC_controller.h" /* Model's header file */
|
||||
#include "rtwtypes.h"
|
||||
|
||||
extern RT_MODEL *const rtM_Left;
|
||||
extern RT_MODEL *const rtM_Right;
|
||||
|
||||
extern DW rtDW_Left; /* Observable states */
|
||||
extern ExtU rtU_Left; /* External inputs */
|
||||
extern ExtY rtY_Left; /* External outputs */
|
||||
|
||||
extern DW rtDW_Right; /* Observable states */
|
||||
extern ExtU rtU_Right; /* External inputs */
|
||||
extern ExtY rtY_Right; /* External outputs */
|
||||
// ###############################################################################
|
||||
|
||||
|
||||
volatile int pwml = 0;
|
||||
volatile int pwmr = 0;
|
||||
|
||||
extern volatile int speed;
|
||||
|
||||
extern volatile adc_buf_t adc_buffer;
|
||||
|
||||
extern volatile uint32_t timeout;
|
||||
@@ -65,6 +77,7 @@ void DMA1_Channel1_IRQHandler() {
|
||||
|
||||
DMA1->IFCR = DMA_IFCR_CTCIF1;
|
||||
// HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
|
||||
// HAL_GPIO_TogglePin(LED_PORT, LED_PIN);
|
||||
|
||||
if(offsetcount < 1000) { // calibrate ADC offsets
|
||||
offsetcount++;
|
||||
@@ -107,17 +120,6 @@ void DMA1_Channel1_IRQHandler() {
|
||||
}
|
||||
|
||||
// ############################### MOTOR CONTROL ###############################
|
||||
int ul, vl, wl;
|
||||
int ur, vr, wr;
|
||||
|
||||
// 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 */
|
||||
@@ -126,44 +128,68 @@ void DMA1_Channel1_IRQHandler() {
|
||||
}
|
||||
OverrunFlag = true;
|
||||
|
||||
int ul, vl, wl;
|
||||
int ur, vr, wr;
|
||||
// ========================= LEFT MOTOR ============================
|
||||
// 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);
|
||||
|
||||
/* Set motor inputs here */
|
||||
rtU.b_hallALeft = hall_ul;
|
||||
rtU.b_hallBLeft = hall_vl;
|
||||
rtU.b_hallCLeft = hall_wl;
|
||||
rtU.r_DCLeft = pwml;
|
||||
|
||||
rtU.b_hallARight = hall_ur;
|
||||
rtU.b_hallBRight = hall_vr;
|
||||
rtU.b_hallCRight = hall_wr;
|
||||
rtU.r_DCRight = pwmr;
|
||||
|
||||
rtU_Left.b_hallA = hall_ul;
|
||||
rtU_Left.b_hallB = hall_vl;
|
||||
rtU_Left.b_hallC = hall_wl;
|
||||
rtU_Left.r_DC = pwml;
|
||||
|
||||
/* Step the controller */
|
||||
BLDC_controller_step();
|
||||
BLDC_controller_step(rtM_Left);
|
||||
|
||||
/* Get motor outputs here */
|
||||
ul = rtY.DC_phaALeft;
|
||||
vl = rtY.DC_phaBLeft;
|
||||
wl = rtY.DC_phaCLeft;
|
||||
// motSpeedLeft = rtY.n_motLeft;
|
||||
// motAngleLeft = rtY.a_elecAngleLeft;
|
||||
ul = rtY_Left.DC_phaA;
|
||||
vl = rtY_Left.DC_phaB;
|
||||
wl = rtY_Left.DC_phaC;
|
||||
// motSpeedLeft = rtY_Left.n_mot;
|
||||
// motAngleLeft = rtY_Left.a_elecAngle;
|
||||
|
||||
ur = rtY.DC_phaARight;
|
||||
vr = rtY.DC_phaBRight;
|
||||
wr = rtY.DC_phaCRight;
|
||||
// motSpeedRight = rtY.n_motRight;
|
||||
// motAngleRight = rtY.a_elecAngleRight;
|
||||
/* Apply commands */
|
||||
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 MOTOR ===========================
|
||||
// Get hall sensors values
|
||||
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);
|
||||
|
||||
/* Set motor inputs here */
|
||||
rtU_Right.b_hallA = hall_ur;
|
||||
rtU_Right.b_hallB = hall_vr;
|
||||
rtU_Right.b_hallC = hall_wr;
|
||||
rtU_Right.r_DC = pwmr;
|
||||
|
||||
/* Step the controller */
|
||||
BLDC_controller_step(rtM_Right);
|
||||
|
||||
/* Get motor outputs here */
|
||||
ur = rtY_Right.DC_phaA;
|
||||
vr = rtY_Right.DC_phaB;
|
||||
wr = rtY_Right.DC_phaC;
|
||||
// motSpeedRight = rtY_Right.n_mot;
|
||||
// motAngleRight = rtY_Right.a_elecAngle;
|
||||
|
||||
/* Apply commands */
|
||||
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);
|
||||
// =================================================================
|
||||
|
||||
/* 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);
|
||||
|
||||
|
||||
// ###############################################################################
|
||||
|
||||
}
|
||||
|
||||
56
Src/main.c
56
Src/main.c
@@ -26,10 +26,27 @@
|
||||
#include <stdlib.h>
|
||||
//#include "hd44780.h"
|
||||
|
||||
// Matlab includes - from auto-code generation
|
||||
#include "BLDC_controller.h" /* Model's header file */
|
||||
// Matlab includes and defines - from auto-code generation
|
||||
// ###############################################################################
|
||||
#include "BLDC_controller.h" /* Model's header file */
|
||||
#include "rtwtypes.h"
|
||||
|
||||
static RT_MODEL rtM_Left_; /* Real-time model */
|
||||
static RT_MODEL rtM_Right_; /* Real-time model */
|
||||
RT_MODEL *const rtM_Left = &rtM_Left_;
|
||||
RT_MODEL *const rtM_Right = &rtM_Right_;
|
||||
|
||||
P rtP; /* Block parameters (auto storage) */
|
||||
|
||||
DW rtDW_Left; /* Observable states */
|
||||
ExtU rtU_Left; /* External inputs */
|
||||
ExtY rtY_Left; /* External outputs */
|
||||
|
||||
DW rtDW_Right; /* Observable states */
|
||||
ExtU rtU_Right; /* External inputs */
|
||||
ExtY rtY_Right; /* External outputs */
|
||||
// ###############################################################################
|
||||
|
||||
void SystemClock_Config(void);
|
||||
|
||||
extern TIM_HandleTypeDef htim_left;
|
||||
@@ -130,18 +147,30 @@ int main(void) {
|
||||
HAL_ADC_Start(&hadc1);
|
||||
HAL_ADC_Start(&hadc2);
|
||||
|
||||
//////////////////////////////////////
|
||||
// Matlab Init
|
||||
// ###############################################################################
|
||||
|
||||
/* Set BLDC controller parameters */
|
||||
/* Set BLDC controller parameters */
|
||||
rtP.z_ctrlTypSel = CTRL_TYP_SEL;
|
||||
rtP.b_phaAdvEna = PHASE_ADV_ENA;
|
||||
rtP.n_commDeacvHi = COMM_DEACV_HI;
|
||||
rtP.n_commAcvLo = COMM_ACV_LO;
|
||||
rtP.b_phaAdvEna = PHASE_ADV_ENA;
|
||||
|
||||
/* Pack LEFT motor data into RTM */
|
||||
rtM_Left->defaultParam = &rtP;
|
||||
rtM_Left->dwork = &rtDW_Left;
|
||||
rtM_Left->inputs = &rtU_Left;
|
||||
rtM_Left->outputs = &rtY_Left;
|
||||
|
||||
/* Initialize BLDC controller */
|
||||
BLDC_controller_initialize();
|
||||
/* Pack RIGHT motor data into RTM */
|
||||
rtM_Right->defaultParam = &rtP;
|
||||
rtM_Right->dwork = &rtDW_Right;
|
||||
rtM_Right->inputs = &rtU_Right;
|
||||
rtM_Right->outputs = &rtY_Right;
|
||||
|
||||
/////////////////////////////////////
|
||||
/* Initialize BLDC controllers */
|
||||
BLDC_controller_initialize(rtM_Left);
|
||||
BLDC_controller_initialize(rtM_Right);
|
||||
|
||||
// ###############################################################################
|
||||
|
||||
for (int i = 8; i >= 0; i--) {
|
||||
buzzerFreq = i;
|
||||
@@ -235,10 +264,11 @@ int main(void) {
|
||||
#endif
|
||||
|
||||
// ####### LOW-PASS FILTER #######
|
||||
cmd2 = cmd2-500;
|
||||
cmd1 = 0;
|
||||
steer = steer * (1.0 - FILTER) + cmd1 * FILTER;
|
||||
speed = speed * (1.0 - FILTER) + cmd2 * FILTER;
|
||||
|
||||
|
||||
// ####### MIXER #######
|
||||
speedR = CLAMP(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT, -1000, 1000);
|
||||
speedL = CLAMP(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT, -1000, 1000);
|
||||
@@ -279,8 +309,8 @@ int main(void) {
|
||||
#endif
|
||||
setScopeChannel(0, (int)speedR); // 1: output command: [-1000, 1000]
|
||||
setScopeChannel(1, (int)speedL); // 2: output command: [-1000, 1000]
|
||||
setScopeChannel(2, (int)rtY.n_motRight); // 3: Real motor speed [rpm]
|
||||
setScopeChannel(3, (int)rtY.n_motLeft); // 4: Real motor speed [rpm]
|
||||
setScopeChannel(2, (int)rtY_Right.n_mot); // 3: Real motor speed [rpm]
|
||||
setScopeChannel(3, (int)rtY_Left.n_mot); // 4: Real motor speed [rpm]
|
||||
setScopeChannel(4, (int)adc_buffer.batt1); // 5: for battery voltage calibration
|
||||
setScopeChannel(5, (int)(batteryVoltage * 100.0f)); // 6: for verifying battery voltage calibration
|
||||
setScopeChannel(6, (int)board_temp_adc_filtered); // 7: for board temperature calibration
|
||||
|
||||
16
Src/setup.c
16
Src/setup.c
@@ -549,7 +549,15 @@ void MX_ADC1_Init(void) {
|
||||
multimode.Mode = ADC_DUALMODE_REGSIMULT;
|
||||
HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode);
|
||||
|
||||
// sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;
|
||||
sConfig.SamplingTime = ADC_SAMPLETIME_7CYCLES_5;
|
||||
// sConfig.SamplingTime = ADC_SAMPLETIME_13CYCLES_5;
|
||||
// sConfig.SamplingTime = ADC_SAMPLETIME_28CYCLES_5;
|
||||
// sConfig.SamplingTime = ADC_SAMPLETIME_41CYCLES_5;
|
||||
// sConfig.SamplingTime = ADC_SAMPLETIME_55CYCLES_5;
|
||||
// sConfig.SamplingTime = ADC_SAMPLETIME_71CYCLES_5;
|
||||
// sConfig.SamplingTime = ADC_SAMPLETIME_239CYCLES_5;
|
||||
|
||||
|
||||
sConfig.Channel = ADC_CHANNEL_14; // pc4 left b
|
||||
sConfig.Rank = 1;
|
||||
@@ -612,7 +620,15 @@ void MX_ADC2_Init(void) {
|
||||
hadc2.Init.NbrOfConversion = 5;
|
||||
HAL_ADC_Init(&hadc2);
|
||||
|
||||
|
||||
// sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;
|
||||
sConfig.SamplingTime = ADC_SAMPLETIME_7CYCLES_5;
|
||||
// sConfig.SamplingTime = ADC_SAMPLETIME_13CYCLES_5;
|
||||
// sConfig.SamplingTime = ADC_SAMPLETIME_28CYCLES_5;
|
||||
// sConfig.SamplingTime = ADC_SAMPLETIME_41CYCLES_5;
|
||||
// sConfig.SamplingTime = ADC_SAMPLETIME_55CYCLES_5;
|
||||
// sConfig.SamplingTime = ADC_SAMPLETIME_71CYCLES_5;
|
||||
// sConfig.SamplingTime = ADC_SAMPLETIME_239CYCLES_5;
|
||||
|
||||
sConfig.Channel = ADC_CHANNEL_15; // pc5 left c
|
||||
sConfig.Rank = 1;
|
||||
|
||||
Reference in New Issue
Block a user