Diff: main.cpp

Revision:

59:568e7be5232f

Parent:

58:fb799e99a5f7

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);                      
     }
 }