Diff: main.cpp

Revision:

5:f1703165ca7e

Parent:

4:e8c1b3f8fc6c

Child:

6:077e37d5c2da

--- a/main.cpp	Thu Apr 01 17:53:19 2021 +0000
+++ b/main.cpp	Thu Apr 01 21:28:37 2021 +0000
@@ -29,9 +29,9 @@
  #define KP_MIN 0.0f
  #define KP_MAX 500.0f
  #define KD_MIN 0.0f
- #define KD_MAX 5.0f
- #define T_MIN -18.0f
- #define T_MAX 18.0f
+ #define KD_MAX 10.0f
+ #define T_MIN -72.0f
+ #define T_MAX 72.0f
  
  /// Joint Soft Stops ///
  #define A_LIM_P 1.5f
@@ -57,13 +57,13 @@
 
 
 Serial       pc(PA_2, PA_3);
-CAN          can1(PB_12, PB_13, 1000000);  // CAN Rx pin name, CAN Tx pin name
-CAN          can2(PA_11, PA_12, 1000000);  // CAN Rx pin name, CAN Tx pin name
-CAN          can3(PA_8, PA_15, 1000000);  // CAN Rx pin name, CAN Tx pin name //CAN4 on board
+CAN          can1(PA_11, PA_12, 1000000);
+CAN          can2(PA_8, PA_15, 1000000);
+CAN          can3(PB_12, PB_13, 1000000); //corresponds to bus 1-3-6 (controller 1) or 2-4-5 (controller 2) IN THAT ORDER
 
 CANMessage   rxMsg1, rxMsg2, rxMsg3;
 CANMessage   txMsg1, txMsg2, txMsg3;
-CANMessage   q11_can, q21_can, q31_can, q12_can, q22_can, q32_can, q13_can, q23_can, q33_can;    //TX Messages
+CANMessage   q11_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;
@@ -77,7 +77,7 @@
 
 
 grouped_act_state g1_state, g2_state, g3_state;
-grouped_act_control q1_control, q2_control, q3_control;
+grouped_act_control g1_control, g2_control, g3_control;
 
 uint16_t x = 0;
 uint16_t x2 = 0;
@@ -159,19 +159,19 @@
     float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
     
     if(id==1){
-        group->a.p = p;
-        group->a.v = v;
-        group->a.t = t;
+        group->a1.p = p;
+        group->a1.v = v;
+        group->a1.t = t;
         }
     else if(id==2){
-        group->h.p = p;
-        group->h.v = v;
-        group->h.t = t;
+        group->a2.p = p;
+        group->a2.v = v;
+        group->a2.t = t;
         }
     else if(id==3){
-        group->k.p = p;
-        group->k.v = v;
-        group->k.t = t;
+        group->a3.p = p;
+        group->a3.v = v;
+        group->a3.t = t;
         }
     } 
 
@@ -189,34 +189,39 @@
     }
     
 void PackAll(){
-    pack_cmd(&q11_can, q1_control.a); 
-    pack_cmd(&q21_can, q1_control.h); 
-    pack_cmd(&q31_can, q1_control.k); 
-    pack_cmd(&q12_can, q2_control.a); 
-    pack_cmd(&q22_can, q2_control.h); 
-    pack_cmd(&q32_can, q2_control.k); 
-    pack_cmd(&q13_can, q3_control.a); 
-    pack_cmd(&q23_can, q3_control.h); 
-    pack_cmd(&q33_can, q3_control.k); 
-    
+    //actuators on the CAN1 bus
+    pack_cmd(&q11_can, q1_control.a1);
+    pack_cmd(&q12_can, q1_control.a2);
+    pack_cmd(&q13_can, q1_control.a3);
+    //actuators on the CAN2 bus
+    pack_cmd(&q21_can, q2_control.a1);
+    pack_cmd(&q22_can, q2_control.a2);
+    pack_cmd(&q23_can, q2_control.a3);
+    //actuators on the CAN3 bus
+    pack_cmd(&q31_can, q3_control.a1);
+    pack_cmd(&q32_can, q3_control.a2);
+    pack_cmd(&q33_can, q3_control.a3);
     }
 void WriteAll(){
     //toggle = 1;
+    //ID = 1 actuators
     can1.write(q11_can);
     wait(.00002);
-    can2.write(q12_can);
+    can2.write(q21_can);
     wait(.00002);
-    can3.write(q13_can);
+    can3.write(q31_can);
     wait(.00002);
-    can1.write(q21_can);
+    //ID = 2 actuators
+    can1.write(q12_can);
     wait(.00002);
     can2.write(q22_can);
     wait(.00002);
-    can3.write(q23_can);
+    can3.write(q32_can);
     wait(.00002);
-    can1.write(q31_can);
+    //ID = 3 actuators
+    can1.write(q13_can);
     wait(.00002);
-    can2.write(q32_can);
+    can2.write(q23_can);
     wait(.00002);
     can3.write(q33_can);
     wait(.00002);
@@ -274,6 +279,7 @@
     msg->data[7] = 0xFD;
     WriteAll();
     }
+
 void serial_isr(){
      /// handle keyboard commands from the serial terminal ///
      while(pc.readable()){
@@ -283,29 +289,38 @@
             case(27):
                 //loop.detach();
                 pc.printf("\n\r exiting motor mode \n\r");
+                //CAN BUS 1
                 ExitMotorMode(&q11_can);
+                ExitMotorMode(&q12_can);
+                ExitMotorMode(&q13_can);
+                //CAN BUS 2
                 ExitMotorMode(&q21_can);
-                ExitMotorMode(&q31_can);
-                ExitMotorMode(&q12_can);
                 ExitMotorMode(&q22_can);
+                ExitMotorMode(&q23_can);
+                //CAN BUS 3
+                ExitMotorMode(&q31_can);
                 ExitMotorMode(&q32_can);
-                ExitMotorMode(&q13_can);
-                ExitMotorMode(&q23_can);
                 ExitMotorMode(&q33_can);
+                //DISABLE FLAG
                 enabled = 0;
                 break;
             case('m'):
                 pc.printf("\n\r entering motor mode \n\r");
+                //CAN BUS 1
                 EnterMotorMode(&q11_can);
+                EnterMotorMode(&q12_can);
+                EnterMotorMode(&q13_can);
+                //CAN BUS 2
                 EnterMotorMode(&q21_can);
-                EnterMotorMode(&q31_can);
-                EnterMotorMode(&q12_can);
                 EnterMotorMode(&q22_can);
+                EnterMotorMode(&q23_can);
+                //CAN BUS 3
+                EnterMotorMode(&q31_can);
                 EnterMotorMode(&q32_can);
-                EnterMotorMode(&q13_can);
-                EnterMotorMode(&q23_can);
                 EnterMotorMode(&q33_can);
+                //WAIT FOR ENABLE
                 wait(.5);
+                //ENABLE FLAG
                 enabled = 1;
                 //loop.attach(&sendCMD, .001);
                 break;
@@ -317,14 +332,17 @@
                 break;
             case('z'):
                 pc.printf("\n\r zeroing \n\r");
+                //CAN BUS 1
                 Zero(&q11_can);
-                Zero(&q21_can);
-                Zero(&q31_can);
                 Zero(&q12_can);
+                Zero(&q13_can);
+                //CAN BUS 2
+                Zero(&q21_can);
                 Zero(&q22_can);
+                Zero(&q23_can);
+                //CAN BUS 3
+                Zero(&q31_can);
                 Zero(&q32_can);
-                Zero(&q13_can);
-                Zero(&q23_can);
                 Zero(&q33_can);
                 break;
             }
@@ -345,60 +363,60 @@
 
 void print_SPI_command() {
     pc.printf("SPI MESSAGE RECIEVED:\n");
-    
-    pc.printf("MOTOR 1 Q: %f\n", spi_command.q_des_1s[0]);
-    pc.printf("MOTOR 1 Qd: %f\n", spi_command.qd_des_1s[0]);
-    pc.printf("MOTOR 1 Kp: %f\n", spi_command.kp_1s[0]);
-    pc.printf("MOTOR 1 Kd: %f\n", spi_command.kd_1s[0]);
-    pc.printf("MOTOR 1 T_FF: %f\n", spi_command.tau_1s_ff[0]);
+    //CAN ONE
+    pc.printf("MOTOR 1-1 Q: %f\n", spi_command.q_des_1s[0]);
+    pc.printf("MOTOR 1-1 Qd: %f\n", spi_command.qd_des_1s[0]);
+    pc.printf("MOTOR 1-1 Kp: %f\n", spi_command.kp_1s[0]);
+    pc.printf("MOTOR 1-1 Kd: %f\n", spi_command.kd_1s[0]);
+    pc.printf("MOTOR 1-1 T_FF: %f\n", spi_command.tau_1s_ff[0]);
     
-    pc.printf("MOTOR 2 Q: %f\n", spi_command.q_des_2s[0]);
-    pc.printf("MOTOR 2 Qd: %f\n", spi_command.qd_des_2s[0]);
-    pc.printf("MOTOR 2 Kp: %f\n", spi_command.kp_2s[0]);
-    pc.printf("MOTOR 2 Kd: %f\n", spi_command.kd_2s[0]);
-    pc.printf("MOTOR 2 T_FF: %f\n", spi_command.tau_2s_ff[0]);
+    pc.printf("MOTOR 1-2 Q: %f\n", spi_command.q_des_2s[0]);
+    pc.printf("MOTOR 1-2 Qd: %f\n", spi_command.qd_des_2s[0]);
+    pc.printf("MOTOR 1-2 Kp: %f\n", spi_command.kp_2s[0]);
+    pc.printf("MOTOR 1-2 Kd: %f\n", spi_command.kd_2s[0]);
+    pc.printf("MOTOR 1-2 T_FF: %f\n", spi_command.tau_2s_ff[0]);
     
-    pc.printf("MOTOR 3 Q: %f\n", spi_command.q_des_3s[0]);
-    pc.printf("MOTOR 3 Qd: %f\n", spi_command.qd_des_3s[0]);
-    pc.printf("MOTOR 3 Kp: %f\n", spi_command.kp_3s[0]);
-    pc.printf("MOTOR 3 Kd: %f\n", spi_command.kd_3s[0]);
-    pc.printf("MOTOR 3 T_FF: %f\n", spi_command.tau_3s_ff[0]);
+    pc.printf("MOTOR 1-3 Q: %f\n", spi_command.q_des_3s[0]);
+    pc.printf("MOTOR 1-3 Qd: %f\n", spi_command.qd_des_3s[0]);
+    pc.printf("MOTOR 1-3 Kp: %f\n", spi_command.kp_3s[0]);
+    pc.printf("MOTOR 1-3 Kd: %f\n", spi_command.kd_3s[0]);
+    pc.printf("MOTOR 1-3 T_FF: %f\n", spi_command.tau_3s_ff[0]);
     
-    pc.printf("MOTOR 4 Q: %f\n", spi_command.q_des_1s[1]);
-    pc.printf("MOTOR 4 Qd: %f\n", spi_command.qd_des_1s[1]);
-    pc.printf("MOTOR 4 Kp: %f\n", spi_command.kp_1s[1]);
-    pc.printf("MOTOR 4 Kd: %f\n", spi_command.kd_1s[1]);
-    pc.printf("MOTOR 4 T_FF: %f\n", spi_command.tau_1s_ff[1]);
+    pc.printf("MOTOR 2-1 Q: %f\n", spi_command.q_des_1s[1]);
+    pc.printf("MOTOR 2-1 Qd: %f\n", spi_command.qd_des_1s[1]);
+    pc.printf("MOTOR 2-1 Kp: %f\n", spi_command.kp_1s[1]);
+    pc.printf("MOTOR 2-1 Kd: %f\n", spi_command.kd_1s[1]);
+    pc.printf("MOTOR 2-1_FF: %f\n", spi_command.tau_1s_ff[1]);
     
-    pc.printf("MOTOR 5 Q: %f\n", spi_command.q_des_2s[1]);
-    pc.printf("MOTOR 5 Qd: %f\n", spi_command.qd_des_2s[1]);
-    pc.printf("MOTOR 5 Kp: %f\n", spi_command.kp_2s[1]);
-    pc.printf("MOTOR 5 Kd: %f\n", spi_command.kd_2s[1]);
-    pc.printf("MOTOR 5 T_FF: %f\n", spi_command.tau_2s_ff[1]);
+    pc.printf("MOTOR 2-2 Q: %f\n", spi_command.q_des_2s[1]);
+    pc.printf("MOTOR 2-2 Qd: %f\n", spi_command.qd_des_2s[1]);
+    pc.printf("MOTOR 2-2 Kp: %f\n", spi_command.kp_2s[1]);
+    pc.printf("MOTOR 2-2 Kd: %f\n", spi_command.kd_2s[1]);
+    pc.printf("MOTOR 2-2 T_FF: %f\n", spi_command.tau_2s_ff[1]);
     
-    pc.printf("MOTOR 6 Q: %f\n", spi_command.q_des_3s[1]);
-    pc.printf("MOTOR 6 Qd: %f\n", spi_command.qd_des_3s[1]);
-    pc.printf("MOTOR 6 Kp: %f\n", spi_command.kp_3s[1]);
-    pc.printf("MOTOR 6 Kd: %f\n", spi_command.kd_3s[1]);
-    pc.printf("MOTOR 6 T_FF: %f\n", spi_command.tau_3s_ff[1]);
+    pc.printf("MOTOR 2-3 Q: %f\n", spi_command.q_des_3s[1]);
+    pc.printf("MOTOR 2-3 Qd: %f\n", spi_command.qd_des_3s[1]);
+    pc.printf("MOTOR 2-3 Kp: %f\n", spi_command.kp_3s[1]);
+    pc.printf("MOTOR 2-3 Kd: %f\n", spi_command.kd_3s[1]);
+    pc.printf("MOTOR 2-3 T_FF: %f\n", spi_command.tau_3s_ff[1]);
     
-    pc.printf("MOTOR 7 Q: %f\n", spi_command.q_des_1s[2]);
-    pc.printf("MOTOR 7 Qd: %f\n", spi_command.qd_des_1s[2]);
-    pc.printf("MOTOR 7 Kp: %f\n", spi_command.kp_1s[2]);
-    pc.printf("MOTOR 7 Kd: %f\n", spi_command.kd_1s[2]);
-    pc.printf("MOTOR 7 T_FF: %f\n", spi_command.tau_1s_ff[2]);
+    pc.printf("MOTOR 3-1 Q: %f\n", spi_command.q_des_1s[2]);
+    pc.printf("MOTOR 3-1 Qd: %f\n", spi_command.qd_des_1s[2]);
+    pc.printf("MOTOR 3-1 Kp: %f\n", spi_command.kp_1s[2]);
+    pc.printf("MOTOR 3-1 Kd: %f\n", spi_command.kd_1s[2]);
+    pc.printf("MOTOR 3-1 T_FF: %f\n", spi_command.tau_1s_ff[2]);
     
-    pc.printf("MOTOR 8 Q: %f\n", spi_command.q_des_2s[2]);
-    pc.printf("MOTOR 8 Qd: %f\n", spi_command.qd_des_2s[2]);
-    pc.printf("MOTOR 8 Kp: %f\n", spi_command.kp_2s[2]);
-    pc.printf("MOTOR 8 Kd: %f\n", spi_command.kd_2s[2]);
-    pc.printf("MOTOR 8 T_FF: %f\n", spi_command.tau_2s_ff[2]);
+    pc.printf("MOTOR 3-2 Q: %f\n", spi_command.q_des_2s[2]);
+    pc.printf("MOTOR 3-2 Qd: %f\n", spi_command.qd_des_2s[2]);
+    pc.printf("MOTOR 3-2 Kp: %f\n", spi_command.kp_2s[2]);
+    pc.printf("MOTOR 3-2 Kd: %f\n", spi_command.kd_2s[2]);
+    pc.printf("MOTOR 3-2 T_FF: %f\n", spi_command.tau_2s_ff[2]);
     
-    pc.printf("MOTOR 9 Q: %f\n", spi_command.q_des_3s[2]);
-    pc.printf("MOTOR 9 Qd: %f\n", spi_command.qd_des_3s[2]);
-    pc.printf("MOTOR 9 Kp: %f\n", spi_command.kp_3s[2]);
-    pc.printf("MOTOR 9 Kd: %f\n", spi_command.kd_3s[2]);
-    pc.printf("MOTOR 9 T_FF: %f\n", spi_command.tau_3s_ff[2]);
+    pc.printf("MOTOR 3-3 Q: %f\n", spi_command.q_des_3s[2]);
+    pc.printf("MOTOR 3-3 Qd: %f\n", spi_command.qd_des_3s[2]);
+    pc.printf("MOTOR 3-3 Kp: %f\n", spi_command.kp_3s[2]);
+    pc.printf("MOTOR 3-3 Kd: %f\n", spi_command.kd_3s[2]);
+    pc.printf("MOTOR 3-3 T_FF: %f\n", spi_command.tau_3s_ff[2]);
     
 }
 
@@ -433,7 +451,7 @@
         ((uint16_t*)(&spi_command))[i] = rx_buff[i];
         //pc.printf("WORD %d RECIEVED: %d\n", i, rx_buff[i]);
     }
-    //print_SPI_command();
+    print_SPI_command();
     
     // run control, which fills in tx_buff for the next iteration
     /*
@@ -443,9 +461,7 @@
         pc.printf("ACTUAL: %d\n", calc_checksum);
         pc.printf("CURRENT: %d\n", spi_command.checksum);}
     */
-        
-    //test_control();
-    //spi_data.q_abad[0] = 12.0f;
+
     control();
     PackAll();
     WriteAll();
@@ -490,24 +506,25 @@
         //BUS ONE
         EnterMotorMode(&q11_can);
         can1.write(q11_can);
+        EnterMotorMode(&q12_can);
+        can1.write(q12_can);
+        EnterMotorMode(&q13_can);
+        can1.write(q13_can);
+        //BUS TWO
         EnterMotorMode(&q21_can);
-        can1.write(q21_can);
-        EnterMotorMode(&q31_can);
-        can1.write(q31_can);
-        //BUS TWO
-        EnterMotorMode(&q12_can);
-        can2.write(q12_can);
+        can2.write(q21_can);
         EnterMotorMode(&q22_can);
         can2.write(q22_can);
-        EnterMotorMode(&q32_can);
-        can2.write(q32_can);
+        EnterMotorMode(&q23_can);
+        can2.write(q23_can);
         //BUS THREE
-        EnterMotorMode(&q13_can);
-        can3.write(q13_can);
-        EnterMotorMode(&q23_can);
-        can3.write(q23_can);
+        EnterMotorMode(&q31_can);
+        can3.write(q31_can);
+        EnterMotorMode(&q32_can);
+        can3.write(q32_can);
         EnterMotorMode(&q33_can);
         can3.write(q33_can);
+        //SERIAL TO USER
         pc.printf("e\n\r");
         return;
     }
@@ -516,53 +533,53 @@
         //BUS ONE
         ExitMotorMode(&q11_can);
         can1.write(q11_can);
+        ExitMotorMode(&q12_can);
+        can1.write(q12_can);
+        ExitMotorMode(&q13_can);
+        can1.write(q13_can);
+        //BUS TWO
         ExitMotorMode(&q21_can);
-        can1.write(q21_can);
-        ExitMotorMode(&q31_can);
-        can1.write(q31_can);
-        //BUS TWO
-        ExitMotorMode(&q12_can);
-        can2.write(q12_can);
+        can2.write(q21_can);
         ExitMotorMode(&q22_can);
         can2.write(q22_can);
-        ExitMotorMode(&q32_can);
-        can2.write(q32_can);
+        ExitMotorMode(&q23_can);
+        can2.write(q23_can);
         //BUS THREE
-        ExitMotorMode(&q13_can);
-        can3.write(q13_can);
-        ExitMotorMode(&q23_can);
-        can3.write(q23_can);
+        ExitMotorMode(&q31_can);
+        can3.write(q31_can);
+        ExitMotorMode(&q32_can);
+        can3.write(q32_can);
         ExitMotorMode(&q33_can);
         can3.write(q33_can);
+        //SERIAL TO USER
         pc.printf("x\n\r");
         return;
         }
     
-    spi_data.q_1s[0] = g1_state.a.p;
-    spi_data.q_2s[0] = g1_state.h.p;
-    spi_data.q_3s[0] = g1_state.k.p;
-    spi_data.qd_1s[0] = g1_state.a.v;
-    spi_data.qd_2s[0] = g1_state.h.v;
-    spi_data.qd_3s[0] = g1_state.k.v;
-    
-    spi_data.q_1s[1] = g1_state.a.p;
-    spi_data.q_2s[1] = g1_state.h.p;
-    spi_data.q_3s[1] = g1_state.k.p;
-    spi_data.qd_1s[1] = g1_state.a.v;
-    spi_data.qd_2s[1] = g1_state.h.v;
-    spi_data.qd_3s[1] = g1_state.k.v;
-    
-    spi_data.q_1s[2] = g1_state.a.p;
-    spi_data.q_2s[2] = g1_state.h.p;
-    spi_data.q_3s[2] = g1_state.k.p;
-    spi_data.qd_1s[2] = g1_state.a.v;
-    spi_data.qd_2s[2] = g1_state.h.v;
-    spi_data.qd_3s[2] = g1_state.k.v;
-    
-    
+    //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.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;
+    //BUS 2 DATA
+    spi_data.q_1s[1] = g1_state.a1.p;
+    spi_data.q_2s[1] = g1_state.a2.p;
+    spi_data.q_3s[1] = g1_state.a3.p;
+    spi_data.qd_1s[1] = g1_state.a1.v;
+    spi_data.qd_2s[1] = g1_state.a2.v;
+    spi_data.qd_3s[1] = g1_state.a3.v;
+    //BUS 3 DATA
+    spi_data.q_1s[2] = g1_state.a1.p;
+    spi_data.q_2s[2] = g1_state.a2.p;
+    spi_data.q_3s[2] = g1_state.a3.p;
+    spi_data.qd_1s[2] = g1_state.a1.v;
+    spi_data.qd_2s[2] = g1_state.a2.v;
+    spi_data.qd_3s[2] = g1_state.a3.v;
     
     if(estop==0){
-        //printf("estopped!!!!\n\r");
+        printf("estopped!!!!\n\r");
         memset(&q1_control, 0, sizeof(q1_control));
         memset(&q2_control, 0, sizeof(q2_control));
         memset(&q3_control, 0, sizeof(q3_control));
@@ -574,78 +591,81 @@
     
     else{
         led = 0;
-        
         memset(&q1_control, 0, sizeof(q1_control));
         memset(&q2_control, 0, sizeof(q2_control));
         memset(&q3_control, 0, sizeof(q3_control));
         
-        
-        q1_control.a.p_des = spi_command.q_des_1s[0];
-        q1_control.a.v_des  = spi_command.qd_des_1s[0];
-        q1_control.a.kp = spi_command.kp_1s[0];
-        q1_control.a.kd = spi_command.kd_1s[0];
-        q1_control.a.t_ff = spi_command.tau_1s_ff[0];
-        
-        q1_control.h.p_des = spi_command.q_des_2s[0];
-        q1_control.h.v_des  = spi_command.qd_des_2s[0];
-        q1_control.h.kp = spi_command.kp_2s[0];
-        q1_control.h.kd = spi_command.kd_2s[0];
-        q1_control.h.t_ff = spi_command.tau_2s_ff[0];
-        
-        q1_control.k.p_des = spi_command.q_des_3s[0];
-        q1_control.k.v_des  = spi_command.qd_des_3s[0];
-        q1_control.k.kp = spi_command.kp_3s[0];
-        q1_control.k.kd = spi_command.kd_3s[0];
-        q1_control.k.t_ff = spi_command.tau_3s_ff[0];
-        
-        q2_control.a.p_des = spi_command.q_des_1s[1];
-        q2_control.a.v_des  = spi_command.qd_des_1s[1];
-        q2_control.a.kp = spi_command.kp_1s[1];
-        q2_control.a.kd = spi_command.kd_1s[1];
-        q2_control.a.t_ff = spi_command.tau_1s_ff[1];
+        //TRANSLATE SPI TO ACTUATOR COMMANNDS
+        //CAN1
+        //CAN1 MOTOR1
+        g1_control.a1.p_des = spi_command.q_des_1s[0];
+        g1_control.a1.v_des  = spi_command.qd_des_1s[0];
+        g1_control.a1.kp = spi_command.kp_1s[0];
+        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];
+        //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];
+        //CAN2
+        //CAN2 MOTOR1
+        g2_control.a1.p_des = spi_command.q_des_1s[1];
+        g2_control.a1.v_des  = spi_command.qd_des_1s[1];
+        g2_control.a1.kp = spi_command.kp_1s[1];
+        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];
+        //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];
+        //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];
+        //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];
+        //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];
         
-        q2_control.h.p_des = spi_command.q_des_2s[1];
-        q2_control.h.v_des  = spi_command.qd_des_2s[1];
-        q2_control.h.kp = spi_command.kp_2s[1];
-        q2_control.h.kd = spi_command.kd_2s[1];
-        q2_control.h.t_ff = spi_command.tau_2s_ff[1];
-        
-        q2_control.k.p_des = spi_command.q_des_3s[1];
-        q2_control.k.v_des  = spi_command.qd_des_3s[1];
-        q2_control.k.kp = spi_command.kp_3s[1];
-        q2_control.k.kd = spi_command.kd_3s[1];
-        q2_control.k.t_ff = spi_command.tau_3s_ff[1];
-        
-        q3_control.a.p_des = spi_command.q_des_1s[2];
-        q3_control.a.v_des  = spi_command.qd_des_1s[2];
-        q3_control.a.kp = spi_command.kp_1s[2];
-        q3_control.a.kd = spi_command.kd_1s[2];
-        q3_control.a.t_ff = spi_command.tau_1s_ff[2];
-        
-        q3_control.h.p_des = spi_command.q_des_2s[2];
-        q3_control.h.v_des  = spi_command.qd_des_2s[2];
-        q3_control.h.kp = spi_command.kp_2s[2];
-        q3_control.h.kd = spi_command.kd_2s[2];
-        q3_control.h.t_ff = spi_command.tau_2s_ff[2];
-        
-        q3_control.k.p_des = spi_command.q_des_3s[2];
-        q3_control.k.v_des  = spi_command.qd_des_3s[2];
-        q3_control.k.kp = spi_command.kp_3s[2];
-        q3_control.k.kd = spi_command.kd_3s[2];
-        q3_control.k.t_ff = spi_command.tau_3s_ff[2];
-        
-        
+        //SPI FLAGS RETURN                                                                      //MIGHT NEED FIXING LATER WHEN WE DO RETURN DATA
         spi_data.flags[0] = 0;
         spi_data.flags[1] = 0;
         spi_data.flags[2] = 0;
-        spi_data.flags[0] |= softstop_joint(g1_state.a, &q1_control.a, A_LIM_P, A_LIM_N);
-        spi_data.flags[0] |= (softstop_joint(g1_state.h, &q1_control.h, H_LIM_P, H_LIM_N))<<1;
+        spi_data.flags[0] |= softstop_joint(g1_state.a1, &g1_control.a1, A_LIM_P, A_LIM_N);
+        spi_data.flags[0] |= (softstop_joint(g1_state.a2, &g1_control.a2, H_LIM_P, H_LIM_N))<<1;
         //spi_data.flags[0] |= (softstop_joint(g1_state.k, &q1_control.k, K_LIM_P, K_LIM_N))<<2;
-        spi_data.flags[1] |= softstop_joint(g2_state.a, &q2_control.a, A_LIM_P, A_LIM_N);
-        spi_data.flags[1] |= (softstop_joint(g2_state.h, &q2_control.h, H_LIM_P, H_LIM_N))<<1;
+        spi_data.flags[1] |= softstop_joint(g2_state.a1, &g2_control.a1, A_LIM_P, A_LIM_N);
+        spi_data.flags[1] |= (softstop_joint(g2_state.a2, &g2_control.a2, H_LIM_P, H_LIM_N))<<1;
         //spi_data.flags[1] |= (softstop_joint(g2_state.k, &q2_control.k, K_LIM_P, K_LIM_N))<<2;
-        spi_data.flags[2] |= softstop_joint(g3_state.a, &q3_control.a, A_LIM_P, A_LIM_N);
-        spi_data.flags[2] |= (softstop_joint(g3_state.h, &q3_control.h, H_LIM_P, H_LIM_N))<<1;
+        spi_data.flags[2] |= softstop_joint(g3_state.a1, &g3_control.a1, A_LIM_P, A_LIM_N);
+        spi_data.flags[2] |= (softstop_joint(g3_state.a2, &g3_control.a2, H_LIM_P, H_LIM_N))<<1;
         //spi_data.flags[2] |= (softstop_joint(g3_state.k, &q3_control.k, K_LIM_P, K_LIM_N))<<2;
         
         //spi_data.flags[0] = 0xbeef;
@@ -686,7 +706,7 @@
 void init_spi(void){
     SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
     spi->format(16, 0);
-    spi->frequency(12000000);
+    spi->frequency(6000000);
     spi->reply(0x0);
     cs.fall(&spi_isr);
     pc.printf("done\n\r");
@@ -723,38 +743,49 @@
     pc.printf("\n\r SPIne\n\r");
     //printf("%d\n\r", RX_ID << 18);
     
+    //CAN 1 BUS
     q11_can.len = 8;                         //transmit 8 bytes
+    q12_can.len = 8;                         //transmit 8 bytes
+    q13_can.len = 8;
+    //CAN 2 BUS
     q21_can.len = 8;                         //transmit 8 bytes
-    q31_can.len = 8;
-    q12_can.len = 8;                         //transmit 8 bytes
     q22_can.len = 8;                         //transmit 8 bytes
-    q32_can.len = 8;
-    q13_can.len = 8;                         //transmit 8 bytes
-    q23_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;
+    //RECIEVE
     rxMsg1.len = 6;                          //receive 6 bytes
     rxMsg2.len = 6;
     rxMsg3.len = 6;
-
+    
+    //CAN 1 BUS
     q11_can.id = 0x1;
-    q21_can.id = 0x2;
-    q31_can.id = 0x3;
-    q12_can.id = 0x1;
+    q12_can.id = 0x2;
+    q13_can.id = 0x3; 
+    //CAN 2 BUS
+    q21_can.id = 0x1;
     q22_can.id = 0x2;
-    q32_can.id = 0x3;
-    q13_can.id = 0x1;
-    q23_can.id = 0x2;
-    q33_can.id = 0x3;    
+    q23_can.id = 0x3; 
+    //CAN 3 BUS
+    q31_can.id = 0x1;
+    q32_can.id = 0x2;
+    q33_can.id = 0x3; 
 
-    pack_cmd(&q11_can, q1_control.a); 
-    pack_cmd(&q12_can, q2_control.a); 
-    pack_cmd(&q13_can, q3_control.a); 
-    pack_cmd(&q21_can, q1_control.h); 
-    pack_cmd(&q22_can, q2_control.h);
-    pack_cmd(&q23_can, q3_control.h); 
-    pack_cmd(&q31_can, q1_control.k); 
-    pack_cmd(&q32_can, q2_control.k);
-    pack_cmd(&q33_can, q3_control.k); 
+    //actuators on the CAN1 bus
+    pack_cmd(&q11_can, q1_control.a1);
+    pack_cmd(&q12_can, q1_control.a2);
+    pack_cmd(&q13_can, q1_control.a3);
+    //actuators on the CAN2 bus
+    pack_cmd(&q21_can, q2_control.a1);
+    pack_cmd(&q22_can, q2_control.a2);
+    pack_cmd(&q23_can, q2_control.a3);
+    //actuators on the CAN3 bus
+    pack_cmd(&q31_can, q3_control.a1);
+    pack_cmd(&q32_can, q3_control.a2);
+    pack_cmd(&q33_can, q3_control.a3);
+    //WRITE THE INITIAL COMMAND 
     WriteAll();
     
     //just debugging things