Revision 15:56a06363a698, committed 2022-09-28

Comitter:

adimmit

Date:

Wed Sep 28 16:32:46 2022 +0000

Parent:

14:6fec697f62ef

Commit message:

updated to work with checksum and with the 2-motor dyno;

Changed in this revision

diff -r 6fec697f62ef -r 56a06363a698 leg_message.h
--- a/leg_message.h	Fri Feb 04 22:38:23 2022 +0000
+++ b/leg_message.h	Wed Sep 28 16:32:46 2022 +0000
@@ -3,43 +3,27 @@
 
 #include <stdint.h>
 
-// 31*4 bytes = 124 8-bit words
-// 31 32-bit numbers
+// 9*2 bytes = 18 16-bit words 36 8-bit words
+// 9 32-bit numbers
 struct spi_data_t
 {
-    float q_1s[3];
-    float q_2s[3];
-    float q_3s[3];
-    float qd_1s[3];
-    float qd_2s[3];
-    float qd_3s[3];
-    float tau_1s[3];
-    float tau_2s[3];
-    float tau_3s[3];
-    uint32_t flags[3];
+    float q_1s[2];
+    float qd_1s[2];
+    float tau_1s[2];
+    int32_t flags[2];
     uint32_t checksum;
 };
 
-// 49*4 bytes = 196 8-bit words
-// 49 32-bit numbers
+// 13*2 bytes = 26 16-bit words 52 8-bit words
+// 13 32-bit numbers
 struct spi_command_t
 {
-    float q_des_1s[3]; //first motor on each can bus 0-can1, 1-can2, 2-can3
-    float q_des_2s[3];
-    float q_des_3s[3];
-    float qd_des_1s[3];
-    float qd_des_2s[3];
-    float qd_des_3s[3];
-    float kp_1s[3];
-    float kp_2s[3];
-    float kp_3s[3];
-    float kd_1s[3];
-    float kd_2s[3];
-    float kd_3s[3];
-    float tau_1s_ff[3];
-    float tau_2s_ff[3];
-    float tau_3s_ff[3];
-    uint32_t flags[3];
+    float q_des_1s[2]; //first motor on each can bus 0-can1, 1-can2, 2-can3
+    float qd_des_1s[2];
+    float kp_1s[2];
+    float kd_1s[2];
+    float tau_1s_ff[2];
+    int32_t flags[2];
     uint32_t checksum;
 };
 
@@ -54,10 +38,10 @@
     };
     
 struct grouped_act_state{
-    joint_state a1, a2, a3; //removed a2 and a3
+    joint_state a1; //removed a2 and a3
     };
 struct grouped_act_control{
-    joint_control a1, a2, a3;
+    joint_control a1;
     }
     ;
 #endif
\ No newline at end of file

diff -r 6fec697f62ef -r 56a06363a698 main.cpp
--- a/main.cpp	Fri Feb 04 22:38:23 2022 +0000
+++ b/main.cpp	Wed Sep 28 16:32:46 2022 +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 62                                                                   //CHECK THESE BUFFER LENGHTS
+#define RX_LEN 52                                                                   //CHECK THESE BUFFER LENGHTS
+#define TX_LEN 52                                                                   //CHECK THESE BUFFER LENGHTS
 
 // length of outgoing/incoming messages
-#define DATA_LEN 62                                                                 //CHECK THESE BUFFER LENGHTS
-#define CMD_LEN  98                                                                 //CHECK THESE BUFFER LENGHTS
+#define DATA_LEN 36                                                                 //CHECK THESE BUFFER LENGHTS
+#define CMD_LEN  52                                                                 //CHECK THESE BUFFER LENGHTS
 
 // Master CAN ID ///
 #define CAN_ID 0x0
@@ -64,7 +64,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, q22_can, q23_can, q31_can, q32_can, q33_can;    //TX Messages
 int                     ledState;
 Ticker                  sendCAN;
 int                     counter = 0;
@@ -77,8 +77,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;
@@ -164,16 +164,6 @@
         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() {
@@ -184,24 +174,12 @@
     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);
     //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);
-    //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);
     }
 void WriteAll(){
     //toggle = 1;
@@ -210,23 +188,6 @@
     wait(.00002);
     can2.write(q21_can);
     wait(.00002);
-    can3.write(q31_can);
-    wait(.00002);
-    //ID = 2 actuators
-    can1.write(q12_can);
-    wait(.00002);
-    can2.write(q22_can);
-    wait(.00002);
-    can3.write(q32_can);
-    wait(.00002);
-    //ID = 3 actuators
-    can1.write(q13_can);
-    wait(.00002);
-    can2.write(q23_can);
-    wait(.00002);
-    can3.write(q33_can);
-    wait(.00002);
-    //toggle = 0;
     }
 
 void sendCMD(){
@@ -235,7 +196,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\n\r", g1_state.a1.p, g2_state.a1.p);
         counter = 0 ;
         }
     
@@ -293,16 +254,8 @@
                 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(&q22_can);
-                ExitMotorMode(&q23_can);
-                //CAN BUS 3
-                ExitMotorMode(&q31_can);
-                ExitMotorMode(&q32_can);
-                ExitMotorMode(&q33_can);
                 //DISABLE FLAG
                 enabled = 0;
                 break;
@@ -310,16 +263,8 @@
                 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(&q22_can);
-                EnterMotorMode(&q23_can);
-                //CAN BUS 3
-                EnterMotorMode(&q31_can);
-                EnterMotorMode(&q32_can);
-                EnterMotorMode(&q33_can);
                 //WAIT FOR ENABLE
                 wait(.5);
                 //ENABLE FLAG
@@ -336,16 +281,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;
             }
         }
@@ -366,61 +311,19 @@
 
 void print_SPI_command() {
     pc.printf("SPI MESSAGE RECIEVED:\n");
-    //CAN ONE
+    //CAN ID 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 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 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 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 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 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 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 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 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]);
-    
 }
 
 
@@ -428,34 +331,13 @@
 
 void print_SPI_data() {
     pc.printf("SPI MESSAGE SENT:\n");
-    //CAN ONE
+    //CAN ID ONES
     pc.printf("MOTOR 1-1 Q: %f\n", spi_data.q_1s[0]);
     pc.printf("MOTOR 1-1 Qd: %f\n", spi_data.qd_1s[0]);
     
-    pc.printf("MOTOR 1-2 Q: %f\n", spi_data.q_2s[0]);
-    pc.printf("MOTOR 1-2 Qd: %f\n", spi_data.qd_2s[0]);
-    
-    pc.printf("MOTOR 1-3 Q: %f\n", spi_data.q_3s[0]);
-    pc.printf("MOTOR 1-3 Qd: %f\n", spi_data.qd_3s[0]);
-    
     pc.printf("MOTOR 2-1 Q: %f\n", spi_data.q_1s[1]);
     pc.printf("MOTOR 2-1 Qd: %f\n", spi_data.qd_1s[1]);
     
-    pc.printf("MOTOR 2-2 Q: %f\n", spi_data.q_2s[1]);
-    pc.printf("MOTOR 2-2 Qd: %f\n", spi_data.qd_2s[1]);
-    
-    pc.printf("MOTOR 2-3 Q: %f\n", spi_data.q_3s[1]);
-    pc.printf("MOTOR 2-3 Qd: %f\n", spi_data.qd_3s[1]);
-    
-    pc.printf("MOTOR 3-1 Q: %f\n", spi_data.q_1s[2]);
-    pc.printf("MOTOR 3-1 Qd: %f\n", spi_data.qd_1s[2]);
-    
-    pc.printf("MOTOR 3-2 Q: %f\n", spi_data.q_2s[2]);
-    pc.printf("MOTOR 3-2 Qd: %f\n", spi_data.qd_2s[2]);
-    
-    pc.printf("MOTOR 3-3 Q: %f\n", spi_data.q_3s[2]);
-    pc.printf("MOTOR 3-3 Qd: %f\n", spi_data.qd_3s[2]);
-    
 }
 
 
@@ -465,40 +347,18 @@
     //pc.printf("%f\n", spi_command.q_des_2s[0]);
     // update qs
     spi_data.q_1s[0] = spi_command.q_des_1s[0]+1.0;
-    spi_data.q_2s[0] = spi_command.q_des_2s[0]+1.0;
-    spi_data.q_3s[0] = spi_command.q_des_3s[0]+1.0;
     spi_data.q_1s[1] = spi_command.q_des_1s[1]+1.0;
-    spi_data.q_2s[1] = spi_command.q_des_2s[1]+1.0;
-    spi_data.q_3s[1] = spi_command.q_des_3s[1]+1.0;
-    spi_data.q_1s[2] = spi_command.q_des_1s[2]+1.0;
-    spi_data.q_2s[2] = spi_command.q_des_2s[2]+1.0;
-    spi_data.q_3s[2] = spi_command.q_des_3s[2]+1.0;
     // update qds
     spi_data.qd_1s[0] = spi_command.qd_des_1s[0]+1.0;
-    spi_data.qd_2s[0] = spi_command.qd_des_2s[0]+1.0;
-    spi_data.qd_3s[0] = spi_command.qd_des_3s[0]+1.0;
     spi_data.qd_1s[1] = spi_command.qd_des_1s[1]+1.0;
-    spi_data.qd_2s[1] = spi_command.qd_des_2s[1]+1.0;
-    spi_data.qd_3s[1] = spi_command.qd_des_3s[1]+1.0;
-    spi_data.qd_1s[2] = spi_command.qd_des_1s[2]+1.0;
-    spi_data.qd_2s[2] = spi_command.qd_des_2s[2]+1.0;
-    spi_data.qd_3s[2] = spi_command.qd_des_3s[2]+1.0;
     // update taus
     spi_data.tau_1s[0] = spi_command.tau_1s_ff[0]+1.0;
-    spi_data.tau_2s[0] = spi_command.tau_2s_ff[0]+1.0;
-    spi_data.tau_3s[0] = spi_command.tau_3s_ff[0]+1.0;
     spi_data.tau_1s[1] = spi_command.tau_1s_ff[1]+1.0;
-    spi_data.tau_2s[1] = spi_command.tau_2s_ff[1]+1.0;
-    spi_data.tau_3s[1] = spi_command.tau_3s_ff[1]+1.0;
-    spi_data.tau_1s[2] = spi_command.tau_1s_ff[2]+1.0;
-    spi_data.tau_2s[2] = spi_command.tau_2s_ff[2]+1.0;
-    spi_data.tau_3s[2] = spi_command.tau_3s_ff[2]+1.0;
     // UDPATE FLAGS
     spi_data.flags[0] = 0;
     spi_data.flags[1] = 0;
-    spi_data.flags[2] = 0;
     // UPDATE CHECKSUM
-    spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 30); 
+    spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 8);  //DONT CRC THE CRC!
     for(int i = 0; i < DATA_LEN; i++){
         tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
 }
@@ -526,7 +386,7 @@
     //update crc_chk from buffer
    for(int i = 0; i < RX_LEN; i++) {((uint16_t*)(&tmp_crc_chk))[i] = rx_buff[i];}
    // CHECK THE CHECKSUM 
-   uint32_t _crc = xor_checksum((uint32_t*)&tmp_crc_chk, 48);
+   uint32_t _crc = xor_checksum((uint32_t*)&tmp_crc_chk, 12); //DONT CRC THE CRC!!!
    // READ CHECKSUM
    uint32_t _rx_crc = tmp_crc_chk.checksum;
     
@@ -538,13 +398,11 @@
        //process();
        control();
        PackAll();
-        WriteAll();
+       WriteAll();
        //new_command = 1;
-    //print_SPI_command();
+       //print_SPI_command();
     }
     else{pc.printf("bigger F in the chat...\n");}
-
- 
 }
 
 int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){
@@ -579,27 +437,9 @@
         //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);
         can2.write(q21_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);
-        //WRITE THE COMMANDS
-        //WriteAll();
-        //SERIAL TO USER
         //pc.printf("e\n\r");
         return;
     }
@@ -608,70 +448,29 @@
         //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);
         can2.write(q21_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);
-        //WRITE THE COMMANDS
-        //WriteAll();
-        //SERIAL TO USER
-        //pc.printf("x\n\r");
         return;
         }
     
     //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;
     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;
     //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.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.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;
-    //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;
     
     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;
         }
     
@@ -679,7 +478,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
@@ -689,78 +488,17 @@
         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];
-        
         //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[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;
-        //spi_data.flags[1] |= softstop_joint(g2_state.a1, &g2_control.a1, A1_LIM_P, A1_LIM_N);
-        //spi_data.flags[1] |= (softstop_joint(g2_state.a2, &g2_control.a2, A2_LIM_P, A2_LIM_N))<<1;
-        //spi_data.flags[1] |= (softstop_joint(g2_state.a3, &g2_control.a3, A3_LIM_P, A3_LIM_N))<<2;
-        //spi_data.flags[2] |= softstop_joint(g3_state.a1, &g3_control.a1, A1_LIM_P, A1_LIM_N);
-        //spi_data.flags[2] |= (softstop_joint(g3_state.a2, &g3_control.a2, A2_LIM_P, A2_LIM_N))<<1;
-        //spi_data.flags[2] |= (softstop_joint(g3_state.a3, &g3_control.a3, A3_LIM_P, A3_LIM_N))<<2;
-        
-        //spi_data.flags[0] = 0xbeef;
-        //spi_data.flags[1] = 0xbeef;
-        //PackAll();
-        //WriteAll();
     }
-    // UPDATE CHECKSUM
-    spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 30); 
+    spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 8);
     for(int i = 0; i < DATA_LEN; i++){
         tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
     
@@ -772,19 +510,15 @@
     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.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.flags[0] = 0xdead;
     //spi_data.flags[1] = 0xbeef;
     
     // only do first 56 bytes of message.
-    spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
+    spi_data.checksum = xor_checksum((uint32_t*)&spi_data,12);
     
     for(int i = 0; i < DATA_LEN; i++)
         tx_buff[i] = ((uint16_t*)(&spi_data))[i];
@@ -793,7 +527,7 @@
 void init_spi(void){
     SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
     spi->format(16, 0);
-    spi->frequency(6000000);
+    spi->frequency(500000);
     spi->reply(0x0);
     cs.fall(&spi_isr);
     pc.printf("done\n\r");
@@ -805,18 +539,9 @@
     pc.baud(115200);                                                                //MAYBE CHANGE THIS IF NEEDED
     pc.attach(&serial_isr);
     estop.mode(PullUp);
-    //spi.format(16, 0);
-    //spi.frequency(1000000);
-    //spi.reply(0x0);
-    //cs.fall(&spi_isr);
 
-    //can1.frequency(1000000);                     // set bit rate to 1Mbps
-    //can1.attach(&rxISR1);                 // attach 'CAN receive-complete' interrupt handler
     can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
-    //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
-    can3.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));
@@ -832,46 +557,21 @@
     
     //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
-    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;
     //RECIEVE
     rxMsg1.len = 6;                          //receive 6 bytes
     rxMsg2.len = 6;
-    rxMsg3.len = 6;
     
     //CAN 1 BUS
     q11_can.id = 0x1;
-    q12_can.id = 0x2;
-    q13_can.id = 0x3; 
     //CAN 2 BUS
     q21_can.id = 0x1;
-    q22_can.id = 0x2;
-    q23_can.id = 0x3; 
-    //CAN 3 BUS
-    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);
     //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);
-    //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);
     //WRITE THE INITIAL COMMAND 
     WriteAll();
     
@@ -891,24 +591,13 @@
     //spi_command=set the thing here... 
             
     while(1) {
-        //pc.printf("test, of SPINE\r\n");
+        //pc.printf("heartbeat...\n");
         counter++;
         can2.read(rxMsg2);
         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);
-        wait_us(10);
-        
-        //print heatbeat (always will print message 0)
-        /*
-        if (counter3 == 100000) {                                                 //for debugging purposes
-            pc.printf("HEARTBEAT\n");
-            counter3 = 0;
-        }
-        counter3++;
-        */
-        }
+        ///wait(0.01);
+    }
 
 }
\ No newline at end of file