Diff: main.cpp

Revision:

12:b203f3ae57d0

Parent:

10:42438194e82b

Child:

13:ee50f3145be7

--- a/main.cpp	Tue May 11 20:17:48 2021 +0000
+++ b/main.cpp	Wed May 12 15:11:14 2021 +0000
@@ -10,12 +10,12 @@
 #include "leg_message.h"
 
 // length of receive/transmit buffers
-#define RX_LEN 98                                                                   //CHECK THESE BUFFER LENGHTS
-#define TX_LEN 98                                                                   //CHECK THESE BUFFER LENGHTS
+#define RX_LEN 26                                                                   //CHECK THESE BUFFER LENGHTS
+#define TX_LEN 26                                                                   //CHECK THESE BUFFER LENGHTS
 
 // length of outgoing/incoming messages
-#define DATA_LEN 44                                                                 //CHECK THESE BUFFER LENGHTS
-#define CMD_LEN  98                                                                 //CHECK THESE BUFFER LENGHTS
+#define DATA_LEN 18                                                                 //CHECK THESE BUFFER LENGHTS
+#define CMD_LEN  26                                                                 //CHECK THESE BUFFER LENGHTS
 
 // Master CAN ID ///
 #define CAN_ID 0x0
@@ -63,7 +63,7 @@
 
 CANMessage   rxMsg1, rxMsg2, rxMsg3;
 CANMessage   txMsg1, txMsg2, txMsg3;
-CANMessage   q11_can, q12_can, q13_can, q21_can, q22_can, q23_can, q31_can, q32_can, q33_can;    //TX Messages
+CANMessage   q11_can, q21_can; //q12_can, q13_can, q21_can, q22_can, q23_can, q31_can, q32_can, q33_can;    //TX Messages
 int                     ledState;
 Ticker                  sendCAN;
 int                     counter = 0;
@@ -76,8 +76,8 @@
 //SPISlave spi(PA_7, PA_6, PA_5, PA_4);
 
 
-grouped_act_state g1_state, g2_state, g3_state;
-grouped_act_control g1_control, g2_control, g3_control;
+grouped_act_state g1_state, g2_state; //, g3_state;
+grouped_act_control g1_control, g2_control; //, g3_control;
 
 uint16_t x = 0;
 uint16_t x2 = 0;
@@ -163,16 +163,20 @@
         group->a1.v = v;
         group->a1.t = t;
         }
+    /*
     else if(id==2){
         group->a2.p = p;
         group->a2.v = v;
         group->a2.t = t;
         }
+    */
+    /*
     else if(id==3){
         group->a3.p = p;
         group->a3.v = v;
         group->a3.t = t;
         }
+    */
     } 
 
  void rxISR1() {
@@ -183,24 +187,26 @@
     can2.read(rxMsg2);
     unpack_reply(rxMsg2, &g2_state);
     }
+/*
 void rxISR3(){
     can3.read(rxMsg3);
     unpack_reply(rxMsg3, &g3_state);
     }
+*/
     
 void PackAll(){
     //actuators on the CAN1 bus
     pack_cmd(&q11_can, g1_control.a1);
-    pack_cmd(&q12_can, g1_control.a2);
-    pack_cmd(&q13_can, g1_control.a3);
+    //pack_cmd(&q12_can, g1_control.a2);
+    //pack_cmd(&q13_can, g1_control.a3);
     //actuators on the CAN2 bus
     pack_cmd(&q21_can, g2_control.a1);
-    pack_cmd(&q22_can, g2_control.a2);
-    pack_cmd(&q23_can, g2_control.a3);
+    //pack_cmd(&q22_can, g2_control.a2);
+    //pack_cmd(&q23_can, g2_control.a3);
     //actuators on the CAN3 bus
-    pack_cmd(&q31_can, g3_control.a1);
-    pack_cmd(&q32_can, g3_control.a2);
-    pack_cmd(&q33_can, g3_control.a3);
+    //pack_cmd(&q31_can, g3_control.a1);
+    //pack_cmd(&q32_can, g3_control.a2);
+    //pack_cmd(&q33_can, g3_control.a3);
     }
 void WriteAll(){
     //toggle = 1;
@@ -209,6 +215,7 @@
     wait(.00002);
     can2.write(q21_can);
     wait(.00002);
+    /*
     can3.write(q31_can);
     wait(.00002);
     //ID = 2 actuators
@@ -226,6 +233,7 @@
     can3.write(q33_can);
     wait(.00002);
     //toggle = 0;
+    */
     }
 
 void sendCMD(){
@@ -234,7 +242,7 @@
     PackAll();
 
     if(counter>100){
-        pc.printf("%.3f %.3f %.3f   %.3f %.3f %.3f   %.3f %.3f %.3f\n\r", g1_state.a1.p, g1_state.a2.p, g1_state.a3.p, g2_state.a1.p, g2_state.a2.p, g2_state.a3.p, g3_state.a1.p, g3_state.a2.p, g3_state.a3.p);
+        //pc.printf("%.3f %.3f %.3f   %.3f %.3f %.3f   %.3f %.3f %.3f\n\r", g1_state.a1.p, g1_state.a2.p, g1_state.a3.p, g2_state.a1.p, g2_state.a2.p, g2_state.a3.p, g3_state.a1.p, g3_state.a2.p, g3_state.a3.p);
         counter = 0 ;
         }
     
@@ -292,16 +300,16 @@
                 pc.printf("\n\r exiting motor mode \n\r");
                 //CAN BUS 1
                 ExitMotorMode(&q11_can);
-                ExitMotorMode(&q12_can);
-                ExitMotorMode(&q13_can);
+                //ExitMotorMode(&q12_can);
+                //ExitMotorMode(&q13_can);
                 //CAN BUS 2
                 ExitMotorMode(&q21_can);
-                ExitMotorMode(&q22_can);
-                ExitMotorMode(&q23_can);
+                //ExitMotorMode(&q22_can);
+                //ExitMotorMode(&q23_can);
                 //CAN BUS 3
-                ExitMotorMode(&q31_can);
-                ExitMotorMode(&q32_can);
-                ExitMotorMode(&q33_can);
+                //ExitMotorMode(&q31_can);
+                //ExitMotorMode(&q32_can);
+                //ExitMotorMode(&q33_can);
                 //DISABLE FLAG
                 enabled = 0;
                 break;
@@ -309,16 +317,16 @@
                 pc.printf("\n\r entering motor mode \n\r");
                 //CAN BUS 1
                 EnterMotorMode(&q11_can);
-                EnterMotorMode(&q12_can);
-                EnterMotorMode(&q13_can);
+                //EnterMotorMode(&q12_can);
+                //EnterMotorMode(&q13_can);
                 //CAN BUS 2
                 EnterMotorMode(&q21_can);
-                EnterMotorMode(&q22_can);
-                EnterMotorMode(&q23_can);
+                //EnterMotorMode(&q22_can);
+                //EnterMotorMode(&q23_can);
                 //CAN BUS 3
-                EnterMotorMode(&q31_can);
-                EnterMotorMode(&q32_can);
-                EnterMotorMode(&q33_can);
+                //EnterMotorMode(&q31_can);
+                //EnterMotorMode(&q32_can);
+                //EnterMotorMode(&q33_can);
                 //WAIT FOR ENABLE
                 wait(.5);
                 //ENABLE FLAG
@@ -335,16 +343,16 @@
                 pc.printf("\n\r zeroing \n\r");
                 //CAN BUS 1
                 Zero(&q11_can);
-                Zero(&q12_can);
-                Zero(&q13_can);
+                //Zero(&q12_can);
+                //Zero(&q13_can);
                 //CAN BUS 2
                 Zero(&q21_can);
-                Zero(&q22_can);
-                Zero(&q23_can);
+                //Zero(&q22_can);
+                //Zero(&q23_can);
                 //CAN BUS 3
-                Zero(&q31_can);
-                Zero(&q32_can);
-                Zero(&q33_can);
+                //Zero(&q31_can);
+                //Zero(&q32_can);
+                //Zero(&q33_can);
                 break;
             }
         }
@@ -362,7 +370,7 @@
 }
 
 
-
+/*
 void print_SPI_command() {
     pc.printf("SPI MESSAGE RECIEVED:\n");
     //CAN ONE
@@ -421,10 +429,10 @@
     pc.printf("MOTOR 3-3 T_FF: %f\n", spi_command.tau_3s_ff[2]);
     
 }
+*/
 
 
-
-
+/*
 void print_SPI_data() {
     pc.printf("SPI MESSAGE SENT:\n");
     //CAN ONE
@@ -456,7 +464,7 @@
     pc.printf("MOTOR 3-3 Qd: %f\n", spi_data.qd_3s[2]);
     
 }
-
+*/
 
 
 
@@ -547,24 +555,24 @@
         //BUS ONE
         EnterMotorMode(&q11_can);
         can1.write(q11_can);
-        EnterMotorMode(&q12_can);
-        can1.write(q12_can);
-        EnterMotorMode(&q13_can);
-        can1.write(q13_can);
+        //EnterMotorMode(&q12_can);
+        //can1.write(q12_can);
+        //EnterMotorMode(&q13_can);
+        //can1.write(q13_can);
         //BUS TWO
         EnterMotorMode(&q21_can);
         can2.write(q21_can);
-        EnterMotorMode(&q22_can);
-        can2.write(q22_can);
-        EnterMotorMode(&q23_can);
-        can2.write(q23_can);
+        //EnterMotorMode(&q22_can);
+        //can2.write(q22_can);
+        //EnterMotorMode(&q23_can);
+        //can2.write(q23_can);
         //BUS THREE
-        EnterMotorMode(&q31_can);
-        can3.write(q31_can);
-        EnterMotorMode(&q32_can);
-        can3.write(q32_can);
-        EnterMotorMode(&q33_can);
-        can3.write(q33_can);
+        //EnterMotorMode(&q31_can);
+        //can3.write(q31_can);
+        //EnterMotorMode(&q32_can);
+        //can3.write(q32_can);
+        //EnterMotorMode(&q33_can);
+        //can3.write(q33_can);
         //WRITE THE COMMANDS
         //WriteAll();
         //SERIAL TO USER
@@ -576,24 +584,24 @@
         //BUS ONE
         ExitMotorMode(&q11_can);
         can1.write(q11_can);
-        ExitMotorMode(&q12_can);
-        can1.write(q12_can);
-        ExitMotorMode(&q13_can);
-        can1.write(q13_can);
+        //ExitMotorMode(&q12_can);
+        //can1.write(q12_can);
+        //ExitMotorMode(&q13_can);
+        //can1.write(q13_can);
         //BUS TWO
         ExitMotorMode(&q21_can);
         can2.write(q21_can);
-        ExitMotorMode(&q22_can);
-        can2.write(q22_can);
-        ExitMotorMode(&q23_can);
-        can2.write(q23_can);
+        //ExitMotorMode(&q22_can);
+        //can2.write(q22_can);
+        //ExitMotorMode(&q23_can);
+        //can2.write(q23_can);
         //BUS THREE
-        ExitMotorMode(&q31_can);
-        can3.write(q31_can);
-        ExitMotorMode(&q32_can);
-        can3.write(q32_can);
-        ExitMotorMode(&q33_can);
-        can3.write(q33_can);
+        //ExitMotorMode(&q31_can);
+        //can3.write(q31_can);
+        //ExitMotorMode(&q32_can);
+        //can3.write(q32_can);
+        //ExitMotorMode(&q33_can);
+        //can3.write(q33_can);
         //WRITE THE COMMANDS
         //WriteAll();
         //SERIAL TO USER
@@ -603,43 +611,43 @@
     
     //BUS 1 DATA
     spi_data.q_1s[0] = g1_state.a1.p;
-    spi_data.q_2s[0] = g1_state.a2.p;
-    spi_data.q_3s[0] = g1_state.a3.p;
+    //spi_data.q_2s[0] = g1_state.a2.p;
+    //spi_data.q_3s[0] = g1_state.a3.p;
     spi_data.qd_1s[0] = g1_state.a1.v;
-    spi_data.qd_2s[0] = g1_state.a2.v;
-    spi_data.qd_3s[0] = g1_state.a3.v;
+    //spi_data.qd_2s[0] = g1_state.a2.v;
+    //spi_data.qd_3s[0] = g1_state.a3.v;
     spi_data.tau_1s[0] = g1_state.a1.t;
-    spi_data.tau_2s[0] = g1_state.a2.t;
-    spi_data.tau_3s[0] = g1_state.a3.t;
+    //spi_data.tau_2s[0] = g1_state.a2.t;
+    //spi_data.tau_3s[0] = g1_state.a3.t;
     //BUS 2 DATA
     spi_data.q_1s[1] = g2_state.a1.p;
-    spi_data.q_2s[1] = g2_state.a2.p;
-    spi_data.q_3s[1] = g2_state.a3.p;
+    //spi_data.q_2s[1] = g2_state.a2.p;
+    //spi_data.q_3s[1] = g2_state.a3.p;
     spi_data.qd_1s[1] = g2_state.a1.v;
-    spi_data.qd_2s[1] = g2_state.a2.v;
-    spi_data.qd_3s[1] = g2_state.a3.v;
+    //spi_data.qd_2s[1] = g2_state.a2.v;
+    //spi_data.qd_3s[1] = g2_state.a3.v;
     spi_data.tau_1s[1] = g2_state.a1.t;
-    spi_data.tau_2s[1] = g2_state.a2.t;
-    spi_data.tau_3s[1] = g2_state.a3.t;
+    //spi_data.tau_2s[1] = g2_state.a2.t;
+    //spi_data.tau_3s[1] = g2_state.a3.t;
     //BUS 3 DATA
-    spi_data.q_1s[2] = g3_state.a1.p;
-    spi_data.q_2s[2] = g3_state.a2.p;
-    spi_data.q_3s[2] = g3_state.a3.p;
-    spi_data.qd_1s[2] = g3_state.a1.v;
-    spi_data.qd_2s[2] = g3_state.a2.v;
-    spi_data.qd_3s[2] = g3_state.a3.v;
-    spi_data.tau_1s[2] = g3_state.a1.t;
-    spi_data.tau_2s[2] = g3_state.a2.t;
-    spi_data.tau_3s[2] = g3_state.a3.t;
+    //spi_data.q_1s[2] = g3_state.a1.p;
+    //spi_data.q_2s[2] = g3_state.a2.p;
+    //spi_data.q_3s[2] = g3_state.a3.p;
+    //spi_data.qd_1s[2] = g3_state.a1.v;
+    //spi_data.qd_2s[2] = g3_state.a2.v;
+    //spi_data.qd_3s[2] = g3_state.a3.v;
+    //spi_data.tau_1s[2] = g3_state.a1.t;
+    //spi_data.tau_2s[2] = g3_state.a2.t;
+    //spi_data.tau_3s[2] = g3_state.a3.t;
     
     if(estop==0){
         printf("estopped!!!!\n\r");
         memset(&g1_control, 0, sizeof(g1_control));
         memset(&g2_control, 0, sizeof(g2_control));
-        memset(&g3_control, 0, sizeof(g3_control));
+        //memset(&g3_control, 0, sizeof(g3_control));
         spi_data.flags[0] = 0xdead;
         spi_data.flags[1] = 0xdead;
-        spi_data.flags[2] = 0xdead;
+        //spi_data.flags[2] = 0xdead;
         led = 1;
         }
     
@@ -647,7 +655,7 @@
         led = 0;
         memset(&g1_control, 0, sizeof(g1_control));
         memset(&g2_control, 0, sizeof(g2_control));
-        memset(&g3_control, 0, sizeof(g3_control));
+        //memset(&g3_control, 0, sizeof(g3_control));
         
         //TRANSLATE SPI TO ACTUATOR COMMANNDS
         //CAN1
@@ -658,17 +666,17 @@
         g1_control.a1.kd = spi_command.kd_1s[0];
         g1_control.a1.t_ff = spi_command.tau_1s_ff[0];
         //CAN1 MOTOR 2
-        g1_control.a2.p_des = spi_command.q_des_2s[0];
-        g1_control.a2.v_des  = spi_command.qd_des_2s[0];
-        g1_control.a2.kp = spi_command.kp_2s[0];
-        g1_control.a2.kd = spi_command.kd_2s[0];
-        g1_control.a2.t_ff = spi_command.tau_2s_ff[0];
+        //g1_control.a2.p_des = spi_command.q_des_2s[0];
+        //g1_control.a2.v_des  = spi_command.qd_des_2s[0];
+        //g1_control.a2.kp = spi_command.kp_2s[0];
+        //g1_control.a2.kd = spi_command.kd_2s[0];
+        //g1_control.a2.t_ff = spi_command.tau_2s_ff[0];
         //CAN1 MOTOR 3
-        g1_control.a3.p_des = spi_command.q_des_3s[0];
-        g1_control.a3.v_des  = spi_command.qd_des_3s[0];
-        g1_control.a3.kp = spi_command.kp_3s[0];
-        g1_control.a3.kd = spi_command.kd_3s[0];
-        g1_control.a3.t_ff = spi_command.tau_3s_ff[0];
+        //g1_control.a3.p_des = spi_command.q_des_3s[0];
+        //g1_control.a3.v_des  = spi_command.qd_des_3s[0];
+        //g1_control.a3.kp = spi_command.kp_3s[0];
+        //g1_control.a3.kd = spi_command.kd_3s[0];
+        //g1_control.a3.t_ff = spi_command.tau_3s_ff[0];
         //CAN2
         //CAN2 MOTOR1
         g2_control.a1.p_des = spi_command.q_des_1s[1];
@@ -677,41 +685,41 @@
         g2_control.a1.kd = spi_command.kd_1s[1];
         g2_control.a1.t_ff = spi_command.tau_1s_ff[1];
         //CAN2 MOTOR 2
-        g2_control.a2.p_des = spi_command.q_des_2s[1];
-        g2_control.a2.v_des  = spi_command.qd_des_2s[1];
-        g2_control.a2.kp = spi_command.kp_2s[1];
-        g2_control.a2.kd = spi_command.kd_2s[1];
-        g2_control.a2.t_ff = spi_command.tau_2s_ff[1];
+        //g2_control.a2.p_des = spi_command.q_des_2s[1];
+        //g2_control.a2.v_des  = spi_command.qd_des_2s[1];
+        //g2_control.a2.kp = spi_command.kp_2s[1];
+        //g2_control.a2.kd = spi_command.kd_2s[1];
+        //g2_control.a2.t_ff = spi_command.tau_2s_ff[1];
         //CAN2 MOTOR 3
-        g2_control.a3.p_des = spi_command.q_des_3s[1];
-        g2_control.a3.v_des  = spi_command.qd_des_3s[1];
-        g2_control.a3.kp = spi_command.kp_3s[1];
-        g2_control.a3.kd = spi_command.kd_3s[1];
-        g2_control.a3.t_ff = spi_command.tau_3s_ff[1];
+        //g2_control.a3.p_des = spi_command.q_des_3s[1];
+        //g2_control.a3.v_des  = spi_command.qd_des_3s[1];
+        //g2_control.a3.kp = spi_command.kp_3s[1];
+        //g2_control.a3.kd = spi_command.kd_3s[1];
+        //g2_control.a3.t_ff = spi_command.tau_3s_ff[1];
         //CAN3
         //CAN3 MOTOR1
-        g3_control.a1.p_des = spi_command.q_des_1s[2];
-        g3_control.a1.v_des  = spi_command.qd_des_1s[2];
-        g3_control.a1.kp = spi_command.kp_1s[2];
-        g3_control.a1.kd = spi_command.kd_1s[2];
-        g3_control.a1.t_ff = spi_command.tau_1s_ff[2];
+        //g3_control.a1.p_des = spi_command.q_des_1s[2];
+        //g3_control.a1.v_des  = spi_command.qd_des_1s[2];
+        //g3_control.a1.kp = spi_command.kp_1s[2];
+        //g3_control.a1.kd = spi_command.kd_1s[2];
+        //g3_control.a1.t_ff = spi_command.tau_1s_ff[2];
         //CAN3 MOTOR 2
-        g3_control.a2.p_des = spi_command.q_des_2s[2];
-        g3_control.a2.v_des  = spi_command.qd_des_2s[2];
-        g3_control.a2.kp = spi_command.kp_2s[2];
-        g3_control.a2.kd = spi_command.kd_2s[2];
-        g3_control.a2.t_ff = spi_command.tau_2s_ff[2];
+        //g3_control.a2.p_des = spi_command.q_des_2s[2];
+        //g3_control.a2.v_des  = spi_command.qd_des_2s[2];
+        //g3_control.a2.kp = spi_command.kp_2s[2];
+        //g3_control.a2.kd = spi_command.kd_2s[2];
+        //g3_control.a2.t_ff = spi_command.tau_2s_ff[2];
         //CAN3 MOTOR 3
-        g3_control.a3.p_des = spi_command.q_des_3s[2];
-        g3_control.a3.v_des  = spi_command.qd_des_3s[2];
-        g3_control.a3.kp = spi_command.kp_3s[2];
-        g3_control.a3.kd = spi_command.kd_3s[2];
-        g3_control.a3.t_ff = spi_command.tau_3s_ff[2];
+        //g3_control.a3.p_des = spi_command.q_des_3s[2];
+        //g3_control.a3.v_des  = spi_command.qd_des_3s[2];
+        //g3_control.a3.kp = spi_command.kp_3s[2];
+        //g3_control.a3.kd = spi_command.kd_3s[2];
+        //g3_control.a3.t_ff = spi_command.tau_3s_ff[2];
         
         //SPI FLAGS RETURN                                                                      //IMPLEMENTS THE JOINT SOFT STOP RIGHT HERE
         spi_data.flags[0] = 0;
         spi_data.flags[1] = 0;
-        spi_data.flags[2] = 0;
+        //spi_data.flags[2] = 0;
         //spi_data.flags[0] |= softstop_joint(g1_state.a1, &g1_control.a1, A1_LIM_P, A1_LIM_N);
         //spi_data.flags[0] |= (softstop_joint(g1_state.a2, &g1_control.a2, A2_LIM_P, A2_LIM_N))<<1;
         //spi_data.flags[0] |= (softstop_joint(g1_state.a3, &g1_control.a3, A3_LIM_P, A3_LIM_N))<<2;
@@ -739,12 +747,12 @@
     for(int i = 0; i < 3; i++)
     {
         spi_data.q_1s[i] = spi_command.q_des_1s[i] + 1.f;
-        spi_data.q_2s[i] = spi_command.q_des_2s[i] + 1.f;
-        spi_data.q_3s[i]  = spi_command.q_des_3s[i]  + 1.f;
+        //spi_data.q_2s[i] = spi_command.q_des_2s[i] + 1.f;
+        //spi_data.q_3s[i]  = spi_command.q_des_3s[i]  + 1.f;
         
         spi_data.qd_1s[i] = spi_command.qd_des_1s[i] + 1.f;
-        spi_data.qd_2s[i] = spi_command.qd_des_2s[i] + 1.f;
-        spi_data.qd_3s[i]  = spi_command.qd_des_3s[i]  + 1.f;
+        //spi_data.qd_2s[i] = spi_command.qd_des_2s[i] + 1.f;
+        //spi_data.qd_3s[i]  = spi_command.qd_des_3s[i]  + 1.f;
     }
     
     spi_data.flags[0] = 0xdead;
@@ -799,46 +807,46 @@
     
     //CAN 1 BUS
     q11_can.len = 8;                         //transmit 8 bytes
-    q12_can.len = 8;                         //transmit 8 bytes
-    q13_can.len = 8;
+    //q12_can.len = 8;                         //transmit 8 bytes
+    //q13_can.len = 8;
     //CAN 2 BUS
     q21_can.len = 8;                         //transmit 8 bytes
-    q22_can.len = 8;                         //transmit 8 bytes
-    q23_can.len = 8;
+    //q22_can.len = 8;                         //transmit 8 bytes
+    //q23_can.len = 8;
     //CAN 3 BUS
-    q31_can.len = 8;                         //transmit 8 bytes
-    q32_can.len = 8;                         //transmit 8 bytes
-    q33_can.len = 8;
+    //q31_can.len = 8;                         //transmit 8 bytes
+    //q32_can.len = 8;                         //transmit 8 bytes
+    //q33_can.len = 8;
     //RECIEVE
     rxMsg1.len = 6;                          //receive 6 bytes
     rxMsg2.len = 6;
-    rxMsg3.len = 6;
+    //rxMsg3.len = 6;
     
     //CAN 1 BUS
     q11_can.id = 0x1;
-    q12_can.id = 0x2;
-    q13_can.id = 0x3; 
+    //q12_can.id = 0x2;
+    //q13_can.id = 0x3; 
     //CAN 2 BUS
     q21_can.id = 0x1;
-    q22_can.id = 0x2;
-    q23_can.id = 0x3; 
+    //q22_can.id = 0x2;
+    //q23_can.id = 0x3; 
     //CAN 3 BUS
-    q31_can.id = 0x1;
-    q32_can.id = 0x2;
-    q33_can.id = 0x3; 
+    //q31_can.id = 0x1;
+    //q32_can.id = 0x2;
+    //q33_can.id = 0x3; 
 
     //actuators on the CAN1 bus
     pack_cmd(&q11_can, g1_control.a1);
-    pack_cmd(&q12_can, g1_control.a2);
-    pack_cmd(&q13_can, g1_control.a3);
+    //pack_cmd(&q12_can, g1_control.a2);
+    //pack_cmd(&q13_can, g1_control.a3);
     //actuators on the CAN2 bus
     pack_cmd(&q21_can, g2_control.a1);
-    pack_cmd(&q22_can, g2_control.a2);
-    pack_cmd(&q23_can, g2_control.a3);
+    //pack_cmd(&q22_can, g2_control.a2);
+    //pack_cmd(&q23_can, g2_control.a3);
     //actuators on the CAN3 bus
-    pack_cmd(&q31_can, g3_control.a1);
-    pack_cmd(&q32_can, g3_control.a2);
-    pack_cmd(&q33_can, g3_control.a3);
+    //pack_cmd(&q31_can, g3_control.a1);
+    //pack_cmd(&q32_can, g3_control.a2);
+    //pack_cmd(&q33_can, g3_control.a3);
     //WRITE THE INITIAL COMMAND 
     WriteAll();
     
@@ -864,8 +872,8 @@
         unpack_reply(rxMsg2, &g2_state);
         can1.read(rxMsg1);                    // read message into Rx message storage
         unpack_reply(rxMsg1, &g1_state);
-        can3.read(rxMsg3);                    // read message into Rx message storage
-        unpack_reply(rxMsg3, &g3_state);
+        //can3.read(rxMsg3);                    // read message into Rx message storage
+        //unpack_reply(rxMsg3, &g3_state);
         wait_us(10);
         
         //print heatbeat (always will print message 0)