Diff: main.cpp

Revision:

49:8dcdb8a89d0e

Parent:

47:55bdc4d5096b

Child:

52:8fff6f1a3f50

diff -r f083ea9d1d03 -r 8dcdb8a89d0e main.cpp
--- a/main.cpp	Thu Jan 09 01:42:00 2020 +0000
+++ b/main.cpp	Tue Oct 26 08:56:41 2021 +0000
@@ -66,7 +66,7 @@
         else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFD))){
             state = REST_MODE;
             state_change = 1;
-            gpio.led->write(0);; 
+            gpio.led->write(0); 
             }
         else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE))){
             spi.ZeroPosition();
@@ -395,6 +395,19 @@
 
     
     while(1) {
+        if(state == MOTOR_MODE)
+             {
+          // if(count >= 400){
+//            printf("J: %.3f  Mec: %.3f   Jerr: %.3f   JVerr: %.3f   Kp: %.3f   Kd: %.3f  \n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, (controller.p_des - controller.theta_mech)*57.2957795,(controller.v_des - controller.dtheta_mech)*57.2957795, controller.kp,controller.kd);
+            //printf("Jraw:%.3f   J: %.3f  Mec: %.3f   N: %.3d   Nmod: %.3f   Mmod: %.3f \n\r",controller.theta_joint_raw*57.2957795,controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, controller.Ncycle,controller.Ncycle_mod*57.2957795,controller.Mech_mod*57.2957795);
 
+              //printf("Pdes: %.3f  Vdes: %.3f   Kp: %.3f   Kd: %.3f   Tff: %.3f\n\r",controller.p_des*57.2957795, controller.v_des*57.2957795, controller.kp,controller.kd,controller.t_ff);
+              pc.printf("Vdes: %.3f\n\rPrel: %.3f  Vrel: %.3f   T: %.3f \n\r",controller.v_des,controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
+              //printf("tor: %.3f\n\r",controller.i_q_filt*KT_OUT)
+             // count = 0;
+            
+            //}
+
+        }
     }
 }