Revision 7:e3cff4376669, committed 2020-07-20

Comitter:

shaorui

Date:

Mon Jul 20 01:03:17 2020 +0000

Parent:

6:3e6d09f56278

Commit message:

1

Changed in this revision

diff -r 3e6d09f56278 -r e3cff4376669 main.cpp
--- a/main.cpp	Tue Nov 19 09:00:12 2019 +0000
+++ b/main.cpp	Mon Jul 20 01:03:17 2020 +0000
@@ -136,12 +136,19 @@
 //================HJB=============added==========
 using namespace FastMath;
 volatile float  Init_pos = 0;
+volatile float  Init_pos1 = 0;//shaorui add
 volatile float  Pmag = 1;
+volatile float  Pmag1 = 1;//shaorui add
 volatile float  Pmag_last = 1;
+volatile float  Pmag_last1 = 1;//shaorui add
 volatile float  Tperiod = 25;
+volatile float  Tperiod1= 25;//shaorui add
 volatile float  p_des_HJB=0;
+volatile float  p_des_shaorui=0;//shaorui add
 volatile float  v_des_HJB=0;
-volatile int count_HJB = 0;
+volatile float  v_des_shaorui=0;//shaorui add
+volatile int count_shaorui= 0;
+volatile int count_HJB= 0;  //shaorui add
 //===================HJB end===================                    
 
 
@@ -241,7 +248,8 @@
     } 
 
  void rxISR1() {
-    can1.read(rxMsg1);                    // read message into Rx message storage
+    can1.read(rxMsg1);                           // read message into Rx message storage
+    printf("%c\n",rxMsg1.id);   //shaorui add                
     unpack_reply(rxMsg1, &l1_state);
 }
 void rxISR2(){
@@ -582,6 +590,7 @@
              //===============================================HJB added====================================================//
             Init_pos = spi_data.q_abad[0];  //==== initial the first position
             count_HJB = 0; //for trajectory hjb
+            count_shaorui = 0; //for trajectory 
             enabled = 1;
             EnterMotorMode(&a1_can);
             can1.write(a1_can);
@@ -687,7 +696,7 @@
             Tperiod = uint_to_float(spi_command.qd_des_abad[0], V_MIN, V_MAX, 12);//;
             if (Pmag != Pmag_last){count_HJB = 0;}
             Pmag_last = Pmag;
-            Init_pos = 0;
+            Init_pos = 0; 
             p_des_HJB = Init_pos + Pmag*FastSin(2*PI*count_HJB/(Tperiod*1000));//Pmag*FastSin(2*PI*count/(Tperiod*40000));
             v_des_HJB = 2*PI*Pmag*FastCos(2*PI*count_HJB/(Tperiod*1000))/Tperiod;
               l1_control.a.p_des = p_des_HJB;//uint_to_float(p_des_HJB, -15.f, 15.f, 16);
@@ -696,7 +705,28 @@
                 count_HJB = 0;
             }
             count_HJB++;
-             //========================================HJB end=========================================//     
+             //========================================HJB end=========================================//  
+             
+             
+                //========================================shaorui added  for trajectory input(leg2)=========================================//
+            Pmag1 = uint_to_float(spi_command.q_des_abad[1], P_MIN, P_MAX, 16); 
+            Tperiod1 = uint_to_float(spi_command.qd_des_abad[1], V_MIN, V_MAX, 12);//;
+            if (Pmag1 != Pmag_last1){count_shaorui = 0;}
+            Pmag_last1 = Pmag1;
+            Init_pos1 = 0; 
+            p_des_shaorui = Init_pos1 + Pmag1*FastSin(2*PI*count_shaorui/(Tperiod1*1000));//Pmag*FastSin(2*PI*count/(Tperiod*40000));
+            v_des_shaorui = 2*PI*Pmag1*FastCos(2*PI*count_shaorui/(Tperiod1*1000))/Tperiod1;
+              l1_control.a.p_des = p_des_shaorui;//uint_to_float(p_des_HJB, -15.f, 15.f, 16);
+              l1_control.a.v_des = v_des_shaorui;
+            if(count_shaorui>=(Tperiod*1000)) {
+                count_shaorui= 0;
+            }
+            count_shaorui++;
+             //========================================shaorui end=========================================//     
+             
+             
+             
+                
                 //l1_control.a.p_des = spi_command.q_des_abad[0];
                 //l1_control.a.v_des  = spi_command.qd_des_abad[0];
                 l1_control.a.kp = spi_command.kp_abad[0];
@@ -843,7 +873,6 @@
     can2.frequency(1000000);                     // set bit rate to 1Mbps
     can2.attach(&rxISR2);                 // attach 'CAN receive-complete' interrupt handler
     can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
-    
     memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
     memset(&spi_data, 0, sizeof(spi_data_t));
     memset(&spi_command,0,sizeof(spi_command_t));
@@ -982,7 +1011,7 @@
             if(counter>100){
                               
                 //printf("%.3f %.3f       %.3f %.3f       %.3f %.3f       %.3f %.3f\n\r", l1_control.a.p_des*Deg_rad, l1_state.a.p*Deg_rad, l1_control.h.p_des*Deg_rad, l1_state.h.p*Deg_rad, l1_control.a.v_des*Deg_rad, l1_state.a.v*Deg_rad, l1_control.h.v_des*Deg_rad, l1_state.h.v*Deg_rad);
-                printf("%.3f %.3f    %.3f %.3f    %d\n\r", p_des_HJB*Deg_rad, l1_state.a.p*Deg_rad, v_des_HJB*Deg_rad, l1_state.a.v*Deg_rad, count_HJB);
+               // printf("%.3f %.3f    %.3f %.3f    %d\n\r", p_des_HJB*Deg_rad, l1_state.a.p*Deg_rad, v_des_HJB*Deg_rad, l1_state.a.v*Deg_rad, count_HJB);//shaorui delete
 
                 //printf("%.3f %.3f\n\r", l1_control.h.p_des, g_ArmJointAngleDe[1]);
                 counter = 0 ;