Diff: main.cpp

Revision:

53:32218a36df05

Parent:

52:d4d5e3414865

Child:

54:e201ae25e467

--- a/main.cpp	Sat Aug 08 13:43:20 2020 +0000
+++ b/main.cpp	Tue Sep 15 08:59:03 2020 +0000
@@ -37,7 +37,7 @@
 #include "math.h"
 #include "MA700Sensor.h"
 #include "joint_calibration.h"
-PreferenceWriter prefs(6);
+PreferenceWriter prefs(7);
 //PreferenceWriter prefs(7);
 
 
@@ -158,7 +158,7 @@
 void calibrate(void){
     gpio.enable->write(1);                                                      // Enable gate drive
     gpio.led->write(1);                                                    // Turn on status LED
-    //order_phases(&spi, &gpio, &controller, &prefs);                             // Check phase ordering
+    order_phases(&spi, &gpio, &controller, &prefs);                             // Check phase ordering
     calibrate(&spi, &gpio, &controller, &prefs);                                // Perform calibration procedure
     //j_calibrate(&ma700,&spi, &gpio, &controller, &prefs);
     //j_calibrate(&ma700,&gpio, &controller, &prefs);     
@@ -333,6 +333,7 @@
         else if(J_M_flag == 1)
         {
             controller.theta_mech = Joint_init + (1.0f/GR)*spi.GetMechPosition(); 
+            
             }
         
         
@@ -353,6 +354,7 @@
                 if(state_change){
                     enter_menu_state();
                     wucha=0 ; //shaorui add
+                    test_jointround_flag=0;
                     }
                 break;
             
@@ -447,6 +449,7 @@
         if(c == 27){
                 state = REST_MODE;
                 state_change = 1;
+                test_jointround_flag=0;
                 char_count = 0;
                 cmd_id = 0;
                 gpio.led->write(0);; 
@@ -495,6 +498,7 @@
                         prefs.load(); 
                     spi.SetMechOffset(M_OFFSET);
                     ma700.SetMechOffset(JOINT_M_OFFSET);
+                    J_M_flag=0;
                     printf("\n\r  Saved new M-J J-J zero position:  %.4f %.4f\n\r\n\r", M_OFFSET, JOINT_M_OFFSET);
                   /*
                     for(int i=0;i<300;i++)
@@ -599,7 +603,8 @@
     spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
     spi.SetMechOffset(M_OFFSET);
     
-    pc.baud(460800);//pc.baud(921600); 
+    //pc.baud(460800);//pc.baud(921600); 
+    pc.baud(115000);
     //=====benk 2020 last version===//
     
     spi.Sample(1.0f);
@@ -628,7 +633,7 @@
     memcpy(&lut, &ENCODER_LUT, sizeof(lut));
     spi.WriteLUT(lut);  
     memcpy(&joint, &ENCODER_JOINT , sizeof(joint));
-    spi.WriteLUT(joint);                             
+    ma700.WriteLUT(joint);                             
                                                                // set serial baud rate
     wait(.01);
         printf("MP: %.3f   JP: %.3f\n\r",spi.GetMechPosition(),ma700.GetMechPosition());
@@ -661,9 +666,9 @@
                 controller.v_des = 0;
                 wait(1);
                 controller.p_des=0;
-                controller.v_des = 1.5f;
+                controller.v_des = 1.0f;
                 controller.kp = 0;
-                controller.kd = 5.0f;
+                controller.kd = 2.0f;
                 controller.t_ff = 0;
                 wait(.5); 
                // }
@@ -682,9 +687,9 @@
                 controller.v_des = 0;
                 wait(1);
                 controller.p_des=0;
-                controller.v_des = -1.5f;
+                controller.v_des = -1.0f;
                 controller.kp = 0;
-                controller.kd = 5.0f;
+                controller.kd = 2.0f;
                 controller.t_ff = 0;
                 wait(.5);
                 printf("test position:%.3f\n\r",(1.0f/GR)* spi.GetMechPosition());
@@ -713,8 +718,8 @@
 //            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);
-             // printf("Prel: %.3f  Vrel: %.3f   T: %.3f \n\r",controller.theta_mech*57.2957795, controller.dtheta_mech*57.2957795, controller.i_q_filt*KT_OUT);
+              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);
+              printf("Prel: %.3f  Vrel: %.3f   T: %.3f \n\r",controller.theta_mech*57.2957795, controller.dtheta_mech*57.2957795, controller.i_q_filt*KT_OUT);
               count = 0;
               }