Wooden Jin
bldc driver firmware based on hobbyking cheetah compact
Dependencies: BLDC_V2 mbed-dev-f303 FastPWM3
Revision 48:a74e401a6d84, committed 2021-04-07
- Comitter:
- Wooden
- Date:
- Wed Apr 07 10:12:43 2021 +0000
- Parent:
- 47:f4ecf3e0576a
- Commit message:
- wooden_bldc_test
Changed in this revision
--- a/BLDC_V2.lib Wed May 13 09:53:27 2020 +0000 +++ b/BLDC_V2.lib Wed Apr 07 10:12:43 2021 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/benkatz/code/Hobbyking_Cheetah_Compact/#6cc428f3431d +https://os.mbed.com/users/Wooden/code/BLDC_V2/#f4ecf3e0576a
--- a/Calibration/calibration.cpp Wed May 13 09:53:27 2020 +0000
+++ b/Calibration/calibration.cpp Wed Apr 07 10:12:43 2021 +0000
@@ -19,6 +19,7 @@
float theta_ref = 0;
float theta_actual = 0;
float v_d = .15f; //Put all volts on the D-Axis
+ // float v_d = .08f;
float v_q = 0.0f;
float v_u, v_v, v_w = 0;
float dtc_u, dtc_v, dtc_w = .5f;
@@ -90,7 +91,8 @@
int raw_b[n] = {0};
float theta_ref = 0;
float theta_actual = 0;
- float v_d = .15f; // Put volts on the D-Axis
+ float v_d = .25f; // Put volts on the D-Axis
+ // float v_d = .08f;
float v_q = 0.0f;
float v_u, v_v, v_w = 0;
float dtc_u, dtc_v, dtc_w = .5f;
--- a/Config/current_controller_config.h Wed May 13 09:53:27 2020 +0000 +++ b/Config/current_controller_config.h Wed Apr 07 10:12:43 2021 +0000 @@ -4,15 +4,22 @@ // Current controller/// #define K_D .05f // Loop gain, Volts/Amp #define K_Q .05f // Loop gain, Volts/Amp -//#define K_SCALE 0.0001f // K_loop/Loop BW (Hz) 0.0042 -#define K_SCALE 0.00165f +#define K_SCALE 0.0001f // K_loop/Loop BW (Hz) 0.0042 +//#define K_SCALE 0.00165f +// #define K_SCALE 0.0000000825f // #define K_SCALE 0.0008 //#define K_SCALE 0.0004 #define KI_D 0.0255f // PI zero, in radians per sample #define KI_Q 0.0255f // PI zero, in radians per sample #define V_BUS 24.0f // Volts +// #define V_BUS 48.0f #define OVERMODULATION 1.2f // 1.0 = no overmodulation +#define kp_design 0.6f +#define ki_design 0.086f +// #define alpha 0.86425f // 1/(2piTf+1), fc=1000 +#define alpha 0.76104f // 1 - 2PiDtFc/(2PiDtFc+1), Fc=2k hz + #define D_INT_LIM V_BUS/(K_D*KI_D) // Amps*samples #define Q_INT_LIM V_BUS/(K_Q*KI_Q) // Amps*samples
--- a/Config/hw_config.h Wed May 13 09:53:27 2020 +0000 +++ b/Config/hw_config.h Wed Apr 07 10:12:43 2021 +0000 @@ -6,9 +6,11 @@ #define PIN_W PA_10 #define ENABLE_PIN PB_5 // Enable gate drive pin #define LED PB_0 // LED Pin -#define I_SCALE 0.080566406f // Amps per A/D Count 3.3v/4096/20/0.0005 +// #define I_SCALE 0.080566406f // Amps per A/D Count 3.3v/4096/20/0.0005 +// #define I_SCALE 0.026855f // Amps per A/D Count 3.3v/4096/20/0.0015 for 1.5m omega current shunt +#define I_SCALE 0.040283f // #define V_SCALE 0.00884f // Bus volts per A/D Count -#define V_SCALE 0.015087891 // Bus volts per A/D count, 3.3/4096*(39+2.2)/2.2 +#define V_SCALE 0.015087891f // Bus volts per A/D count, 3.3/4096*(39+2.2)/2.2 #define DTC_MAX 0.95f // Max phase duty cycle #define DTC_MIN 0.05f // Min phase duty cycle #define PWM_ARR 0x8CA /// timer autoreload value
--- a/Config/motor_config.h Wed May 13 09:53:27 2020 +0000 +++ b/Config/motor_config.h Wed Apr 07 10:12:43 2021 +0000 @@ -1,19 +1,24 @@ #ifndef MOTOR_CONFIG_H #define MOTOR_CONFIG_H -#define R_PHASE 0.105f //Ohms +// #define R_PHASE 0.105f //Ohms +#define R_PHASE 0.1265f //#define L_D 0.00003f //Henries -#define L_D 0.00005f +//#define L_D 0.00005f +#define L_D 0.000066f //#define L_Q 0.00003f //Henries -#define L_Q 0.00005f -//#define KT .075f //N-m per peak phase amp, = WB*NPP*3/2 -#define KT .05f +//#define L_Q 0.00005f +#define L_Q 0.000066f +#define KT .075f //N-m per peak phase amp, = WB*NPP*3/2 +// #define KT .05f #define NPP 21 //Number of pole pairs #define GR 6.0f //Gear ratio -// #define KT_OUT 0.45f //KT*GR -#define KT_OUT 0.30f // xmotor, 这个可能是受到了磁铁和保持架影响 -//#define WB 0.0024f //Webers. -#define WB 0.0016f +#define KT_OUT 0.45f //KT*GR +// #define KT_OUT 0.30f +// xmotor, 这个可能是受到了磁铁和保持架影响 +// #define KT_OUT 0.055f +#define WB 0.0024f //Webers. +// #define WB 0.0016f #endif
--- a/FOC/foc.cpp Wed May 13 09:53:27 2020 +0000
+++ b/FOC/foc.cpp Wed Apr 07 10:12:43 2021 +0000
@@ -1,4 +1,5 @@
+// He version
#include "foc.h"
using namespace FastMath;
@@ -81,12 +82,16 @@
/// Commutation Loop ///
controller->loop_count ++;
if(PHASE_ORDER){ // Check current sensor ordering
- controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); // Calculate phase currents from ADC readings
- controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
+ //controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); // Calculate phase currents from ADC readings
+ //controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
+ controller->i_b = alpha * controller->i_b + (1.0f - alpha) * I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); // low pass filter, freq=1-alpha / dt
+ controller->i_c = alpha * controller->i_c + (1.0f - alpha) * I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
}
- else{
- controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
- controller->i_c = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
+ else{
+ // controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
+ // controller->i_c = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
+ controller->i_b = alpha * controller->i_b + (1.0f - alpha) * I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
+ controller->i_c = alpha * controller->i_c + (1.0f - alpha) * I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
}
controller->i_a = -controller->i_b - controller->i_c;
@@ -114,13 +119,14 @@
/// PI Controller ///
// float i_d_error = controller->i_d_ref - controller->i_d;
- float i_d_error = controller->i_d_ref - observer->i_d_est;
-// float i_q_error = controller->i_q_ref - controller->i_q;// + cogging_current*0.6;
- float i_q_error = controller->i_q_ref - observer->i_q_est;// + cogging_current;
- //float i_q_error = controller->i_q_ref - observer->i_q_est;
+ float i_d_error = controller->i_d_ref - observer->i_d_est;
+// float i_q_error = controller->i_q_ref - controller->i_q + cogging_current*0.6;
+// float i_q_error = controller->i_q_ref - controller->i_q;
+ float i_q_error = controller->i_q_ref - observer->i_q_est;// + cogging_current;
+// float i_q_error = controller->i_q_ref - observer->i_q_est;
- float v_d_ff = 2.0f*(controller->i_d_ref*R_PHASE - controller->dtheta_elec*L_Q*controller->i_q_ref); //feed-forward voltages
- float v_q_ff = 2.0f*(controller->i_q_ref*R_PHASE + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));
+ float v_d_ff = (controller->i_d_ref*R_PHASE - controller->dtheta_elec*L_Q*controller->i_q_ref); //feed-forward voltages
+ float v_q_ff = (controller->i_q_ref*R_PHASE + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));
controller->d_int += i_d_error;
controller->q_int += i_q_error;
@@ -128,9 +134,12 @@
//v_d_ff = 0;
//v_q_ff = 0;
- limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_SCALE*I_BW*KI_Q)); // Limit integrators to prevent windup
- controller->v_d = K_SCALE*I_BW*i_d_error + K_SCALE*I_BW*KI_D*controller->d_int+ v_d_ff;
- controller->v_q = K_SCALE*I_BW*i_q_error + K_SCALE*I_BW*KI_Q*controller->q_int+ v_q_ff;
+ // limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_SCALE*I_BW*KI_Q)); // Limit integrators to prevent windup
+ limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_SCALE*I_BW*KI_Q));
+ controller->v_d = K_SCALE*I_BW*i_d_error + K_SCALE*I_BW*KI_D*controller->d_int + v_d_ff;
+ controller->v_q = K_SCALE*I_BW*i_q_error + K_SCALE*I_BW*KI_Q*controller->q_int + v_q_ff;
+ //controller->v_d = kp_design * i_d_error + ki_design * DT * controller->d_int;
+ //controller->v_q = kp_design * i_q_error + ki_design * DT * controller->q_int;
// controller->v_q = 0.0f; // always zero
// controller->v_d = 0.0f;
@@ -146,8 +155,10 @@
//controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - (0.86602540378f*c-.5f*s)*controller->v_q;
svm(controller->v_bus, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
- observer->i_d_dot = 0.5f*(controller->v_d - 2.0f*(observer->i_d_est*R_PHASE - controller->dtheta_elec*L_Q*observer->i_q_est))/L_D; //feed-forward voltage
- observer->i_q_dot = 0.5f*(controller->v_q - 2.0f*(observer->i_q_est*R_PHASE + controller->dtheta_elec*(L_D*observer->i_d_est + WB)))/L_Q;
+ // observer->i_d_dot = 0.5f*(controller->v_d - 2.0f*(observer->i_d_est*R_PHASE - controller->dtheta_elec*L_Q*observer->i_q_est))/L_D; //feed-forward voltage
+ // observer->i_q_dot = 0.5f*(controller->v_q - 2.0f*(observer->i_q_est*R_PHASE + controller->dtheta_elec*(L_D*observer->i_d_est + WB)))/L_Q;
+ observer->i_d_dot = (controller->v_d - (observer->i_d_est*R_PHASE - controller->dtheta_elec*L_Q*observer->i_q_est))/L_D; //feed-forward voltage
+ observer->i_q_dot = (controller->v_q - (observer->i_q_est*R_PHASE + controller->dtheta_elec*(L_D*observer->i_d_est + WB)))/L_Q;
if(PHASE_ORDER){ // Check which phase order to use,
TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u); // Write duty cycles
@@ -177,9 +188,20 @@
void torque_control(ControllerStruct *controller){
+ // controller->t_ff= 0.5f;
+ // controller->kd = 0.1f;
float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech);
+ if(torque_ref>18.0f){
+ torque_ref = 18.0f;
+ }
+ else{
+ if(torque_ref <-18.0f){
+ torque_ref = -18.0f;
+ }
+ }
//float torque_ref = -.1*(controller->p_des - controller->theta_mech);
- controller->i_q_ref = torque_ref/KT_OUT;
+ controller->i_q_ref = torque_ref/KT_OUT;
+ // controller->i_q_ref = 1.0f;
controller->i_d_ref = 0.0f;
}
@@ -188,4 +210,4 @@
void zero_encoder(ControllerStruct *controller, GPIOStruct *gpio, ){
}
-*/
\ No newline at end of file
+*/
--- a/PositionSensor/PositionSensor.cpp Wed May 13 09:53:27 2020 +0000
+++ b/PositionSensor/PositionSensor.cpp Wed Apr 07 10:12:43 2021 +0000
@@ -24,6 +24,10 @@
oldVel = 0;
raw = 0;
flag_first_time = true;
+ for(int i=0;i<40;i++){
+ velVec[i] = 0;
+ // MechPositionVec[i] = 0;
+ }
}
void PositionSensorAM5147::Sample(float dt){
@@ -72,14 +76,20 @@
int n = 40;
float sum = vel;
+ //float sum2 = MechPosition;
for (int i = 1; i < (n); i++){
velVec[n - i] = velVec[n-i-1];
sum += velVec[n-i];
+ //MechPositionVec[n-i] = MechPositionVec[n-i-1];
+ //sum2 += MechPositionVec[n-i];
}
velVec[0] = vel;
+ //MechPositionVec[0] = MechPosition;
+ //MechPosition = sum2 / ((float)n);
MechVelocity = sum/((float)n);
ElecVelocity = MechVelocity*_ppairs;
- ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
+ // ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
+ ElecVelocityFilt = 0.76094f*ElecVelocityFilt + 0.23906f*ElecVelocity; // 2khz speed filter
}
int PositionSensorAM5147::GetRawPosition(){
@@ -99,7 +109,8 @@
}
float PositionSensorAM5147::GetElecVelocity(){
- return ElecVelocity;
+ //return ElecVelocity;
+ return ElecVelocityFilt;
}
float PositionSensorAM5147::GetMechVelocity(){
--- a/PositionSensor/PositionSensor.h Wed May 13 09:53:27 2020 +0000
+++ b/PositionSensor/PositionSensor.h Wed Apr 07 10:12:43 2021 +0000
@@ -57,7 +57,7 @@
virtual int GetCPR(void);
virtual void WriteLUT(int new_lut[128]);
private:
- float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt;
+ float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt, MechPositionVec[40];
int raw, _CPR, rotations, old_counts, _ppairs;
bool flag_first_time; // avoid the first time angle - old_counts
SPI *spi;
--- a/main.cpp Wed May 13 09:53:27 2020 +0000
+++ b/main.cpp Wed Apr 07 10:12:43 2021 +0000
@@ -82,7 +82,9 @@
else if(state == MOTOR_MODE){
unpack_cmd(rxMsg, &controller);
}
- pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
+ // pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
+ pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q*KT_OUT); //return real torque
+// pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.v_bus/2.0f);
can.write(txMsg);
}
@@ -117,6 +119,7 @@
pc.printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
wait_us(10);
pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
+ //pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "1", "100", I_BW);
wait_us(10);
pc.printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
wait_us(10);
@@ -196,10 +199,13 @@
controller.adc3_raw = ADC3->DR;
spi.Sample(DT); // sample position sensor
controller.theta_elec = spi.GetElecPosition();
+// controller.theta_elec = 0;
controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();
controller.dtheta_elec = spi.GetElecVelocity();
- controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
+// controller.dtheta_elec = 0;
+// controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
+ controller.v_bus = 24.0;
///
LLLL ++;
if(LLLL==10000){
@@ -375,6 +381,7 @@
switch (cmd_id){
case 'b':
I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f);
+ //I_BW = fmaxf(fminf(atof(cmd_val), 100.0f), 1.0f);
break;
case 'i':
CAN_ID = atoi(cmd_val);
@@ -469,6 +476,7 @@
controller.v_bus = V_BUS;
controller.mode = 0;
+ // controller.kd = 0.1;
Init_All_HW(&gpio); // Setup PWM, ADC, GPIO
wait(.1);
@@ -502,11 +510,14 @@
memcpy(&lut, &ENCODER_LUT, sizeof(lut));
spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table
- pc.baud(921600); // set serial baud rate
- // pc.baud(460800);
+ // pc.baud(921600); // set serial baud rate
+ pc.baud(460800);
wait(.01);
- pc.printf("\n\r\n\r 3.3, With obs, vdff, no cogging\n\r\n\r");
+ //pc.printf("\n\r\n\r 3.3, With obs, vdff, no cogging\n\r\n\r");
+ // pc.printf("aloha, it do is 48v, low cal");
+ pc.printf("origin control with current filter 2000hz and current shunt is 1.5mo\n\r");
+ pc.printf("vdff_cogging_\n\r");
wait(.01);
pc.printf("\n\r Debug Info:\n\r");
pc.printf(" Firmware Version: %s\n\r", VERSION_NUM);