Diff: main.cpp

Revision:

9:16a550d36425

Parent:

8:450fbccaf4f6

Child:

10:42438194e82b

--- a/main.cpp	Tue Apr 06 00:58:00 2021 +0000
+++ b/main.cpp	Fri Apr 09 20:41:24 2021 +0000
@@ -425,6 +425,41 @@
 
 
 
+void print_SPI_data() {
+    pc.printf("SPI MESSAGE SENT:\n");
+    //CAN ONE
+    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]);
+    
+}
+
+
+
+
 
 void spi_isr(void)
 {
@@ -451,6 +486,7 @@
     for(int i = 0; i < CMD_LEN; i++)
     {
         ((uint16_t*)(&spi_command))[i] = rx_buff[i];
+        //pc.printf("BYTE %d SENT: %d\n", i, tx_buff[i]);
         //pc.printf("WORD %d RECIEVED: %d\n", i, rx_buff[i]);
     }
     //print_SPI_command();
@@ -464,6 +500,7 @@
         pc.printf("CURRENT: %d\n", spi_command.checksum);}
     */
 
+    //print_SPI_data();
     control();
     PackAll();
     WriteAll();
@@ -479,6 +516,7 @@
 }
 
 int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){
+    /*
     if((state.p)>=limit_p){
         //control->p_des = limit_p;
         control->v_des = 0.0f;
@@ -495,6 +533,7 @@
         control->t_ff += KP_SOFTSTOP*(limit_n - state.p);
         return 1;
     }
+    */
     return 0;
     
     }
@@ -570,19 +609,19 @@
     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;
+    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;
     //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;
+    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;
     
     if(estop==0){
         printf("estopped!!!!\n\r");
@@ -664,15 +703,15 @@
         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] |= 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;