nhan pham an
Fork and fix for mwork
Dependencies: mbed-dev-f303 FastPWM3 millis
Revision 59:568e7be5232f, committed 2020-06-29
- Comitter:
- annhandt09
- Date:
- Mon Jun 29 09:36:26 2020 +0000
- Parent:
- 58:fb799e99a5f7
- Commit message:
- add controller.p_des = controller.theta_mech - (controller.t_ff / controller.kp) ; for first enable.
Changed in this revision
diff -r fb799e99a5f7 -r 568e7be5232f FOC/foc.cpp
--- a/FOC/foc.cpp Mon Jun 29 03:34:16 2020 +0000
+++ b/FOC/foc.cpp Mon Jun 29 09:36:26 2020 +0000
@@ -132,7 +132,7 @@
observer->temperature2 = (double)(25.0f + ((observer->resistance*6.0606f)-1.0f)*275.5f);
double e = observer->temperature - observer->temperature2;
observer->temperature -= .001*e;
- //printf("%.3f\n\r", e);
+
/// Commutation Loop ///
@@ -167,6 +167,7 @@
controller->fw_int += .001f*(0.5f*OVERMODULATION*controller->v_bus - controller->v_ref);
controller->fw_int = fmaxf(fminf(controller->fw_int, 0.0f), -I_FW_MAX);
controller->i_d_ref = controller->fw_int;
+
//float i_cmd_mag_sq = controller->i_d_ref*controller->i_d_ref + controller->i_q_ref*controller->i_q_ref;
limit_norm(&controller->i_d_ref, &controller->i_q_ref, I_MAX);
@@ -192,7 +193,7 @@
controller->v_q = controller->k_q*i_q_error + controller->q_int ;//+ v_q_ff;
controller->v_ref = sqrt(controller->v_d*controller->v_d + controller->v_q*controller->v_q);
-
+ // printf("%.3f - %.3f - %.3f \n\r", controller->i_q_filt,controller->i_d_filt,0);
limit_norm(&controller->v_d, &controller->v_q, OVERMODULATION*controller->v_bus); // Normalize voltage vector to lie within curcle of radius v_bus
float dtc_d = controller->v_d/controller->v_bus;
float dtc_q = controller->v_q/controller->v_bus;
@@ -202,7 +203,7 @@
controller->v_q = dtc_q*controller->v_bus;
abc(controller->theta_elec + 0.0f*DT*controller->dtheta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
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
-
+
if(PHASE_ORDER){ // Check which phase order to use,
TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u); // Write duty cycles
TIM1->CCR2 = (PWM_ARR)*(1.0f-controller->dtc_v);
@@ -213,7 +214,7 @@
TIM1->CCR1 = (PWM_ARR)*(1.0f-controller->dtc_v);
TIM1->CCR2 = (PWM_ARR)*(1.0f-controller->dtc_w);
}
-
+
controller->theta_elec = theta;
}
@@ -222,6 +223,7 @@
void torque_control(ControllerStruct *controller){
float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech);
//float torque_ref = -.1*(controller->p_des - controller->theta_mech);
+
controller->i_q_ref = torque_ref/KT_OUT;
controller->i_d_ref = 0.0f;
}
diff -r fb799e99a5f7 -r 568e7be5232f main.cpp
--- a/main.cpp Mon Jun 29 03:34:16 2020 +0000
+++ b/main.cpp Mon Jun 29 09:36:26 2020 +0000
@@ -45,8 +45,8 @@
GPIOStruct gpio;
ControllerStruct controller = {
- .kp = 24.0 ,
- .kd = 0.25 ,
+ .kp = 250.0 ,
+ .kd = 2.5 ,
.t_ff = -17.0
};
ObserverStruct observer;
@@ -288,6 +288,12 @@
cond_printf(" Gear Ratio %.4f:1\r\n", GR);
cond_printf(" Mapped Position %.4f to %.4f Radians\n\r", P_MIN, P_MAX);
cond_printf(" PHASE_ORDER %d \r\n", PHASE_ORDER);
+ cond_printf(" alpha: %.4f Kd: %.4f\n\r", controller.alpha, controller.k_d);
+ cond_printf(" theta_mech: %.4f theta_elec: %.4f\n\r", controller.theta_mech, controller.theta_elec);
+ cond_printf(" dtheta_mech: %.4f Dtheta_elec: %.4f\n\r", controller.dtheta_mech, controller.dtheta_elec);
+ cond_printf(" pdes: %.4f vdes: %.4f\n\r", controller.p_des, controller.v_des);
+ cond_printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);
+ cond_printf("%.3f %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec);
cond_printf(" CAN ID: %d\n\r", CAN_ID);
}
void enter_torque_mode(void){
@@ -360,7 +366,7 @@
case MOTOR_MODE: // Run torque control
if(state_change){
- controller.p_des = controller.theta_mech + 0.710 ;
+ controller.p_des = controller.theta_mech - (controller.t_ff / controller.kp) ;
enter_torque_mode();
count = 0;
}
@@ -424,13 +430,8 @@
gpio.led->write(0);;
for(int i = 0; i<8; i++){cmd_val[i] = 0;}
}
- if(c == 't'){
- cond_printf("Pos: %.3f Vel: %.3f Cur: %.3f\r\n", controller.theta_mech, controller.theta_elec, controller.i_q_filt*KT_OUT);
- }
- else if(c == 'y'){
- printFirmwareVer();
- }
-
+ if(c == 't'){cond_printf("Pos: %.3f Vel: %.3f Cur: %.3f\r\n", controller.theta_mech, controller.theta_elec, controller.i_q_filt*KT_OUT);}
+ else if(c == 'y'){printFirmwareVer();}
if(state == REST_MODE){
switch (c){
case 'c':
@@ -625,12 +626,11 @@
//set_io_mode(IO_MODE_STEP_DIR);
wait_us(1000);
printFirmwareVer();
-
state_change = 1;
timeSche = 0 ;
while(1) {
drv.print_faults();
//sendSche();
- wait(.1);
+ wait(.1);
}
}
diff -r fb799e99a5f7 -r 568e7be5232f structs.h
--- a/structs.h Mon Jun 29 03:34:16 2020 +0000
+++ b/structs.h Mon Jun 29 09:36:26 2020 +0000
@@ -21,7 +21,7 @@
float v_bus; // DC link voltage
float theta_mech, theta_elec; // Rotor mechanical and electrical angle
float dtheta_mech, dtheta_elec, dtheta_elec_filt; // Rotor mechanical and electrical angular velocit
- float i_d, i_q, i_q_filt, i_d_filt; // D/Q currents
+ float i_d, i_q, i_q_filt, i_d_filt; // D/Q currents
float v_d, v_q; // D/Q voltages
float dtc_u, dtc_v, dtc_w; // Terminal duty cycles
float v_u, v_v, v_w; // Terminal voltages