Diff: main.cpp

Revision:

6:077e37d5c2da

Parent:

5:f1703165ca7e

Child:

7:2f785cb39f11

--- a/main.cpp	Thu Apr 01 21:28:37 2021 +0000
+++ b/main.cpp	Thu Apr 01 22:48:48 2021 +0000
@@ -190,17 +190,17 @@
     
 void PackAll(){
     //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);
+    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, q2_control.a1);
-    pack_cmd(&q22_can, q2_control.a2);
-    pack_cmd(&q23_can, q2_control.a3);
+    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, q3_control.a1);
-    pack_cmd(&q32_can, q3_control.a2);
-    pack_cmd(&q33_can, q3_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;
@@ -234,7 +234,7 @@
     PackAll();
 
     if(counter>100){
-        pc.printf("%.3f %.3f %.3f   %.3f %.3f %.3f   %.3f %.3f %.3f\n\r", g1_state.a.p, g1_state.h.p, g1_state.k.p, g2_state.a.p, g2_state.h.p, g2_state.k.p, g3_state.a.p, g3_state.h.p, g3_state.k.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 ;
         }
     
@@ -451,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
     /*
@@ -580,9 +580,9 @@
     
     if(estop==0){
         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));
+        memset(&g1_control, 0, sizeof(g1_control));
+        memset(&g2_control, 0, sizeof(g2_control));
+        memset(&g3_control, 0, sizeof(g3_control));
         spi_data.flags[0] = 0xdead;
         spi_data.flags[1] = 0xdead;
         spi_data.flags[2] = 0xdead;
@@ -591,9 +591,9 @@
     
     else{
         led = 0;
-        memset(&q1_control, 0, sizeof(q1_control));
-        memset(&q2_control, 0, sizeof(q2_control));
-        memset(&q3_control, 0, sizeof(q3_control));
+        memset(&g1_control, 0, sizeof(g1_control));
+        memset(&g2_control, 0, sizeof(g2_control));
+        memset(&g3_control, 0, sizeof(g3_control));
         
         //TRANSLATE SPI TO ACTUATOR COMMANNDS
         //CAN1
@@ -660,7 +660,7 @@
         spi_data.flags[2] = 0;
         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[0] |= (softstop_joint(g1_state.k, &g1_control.k, K_LIM_P, K_LIM_N))<<2;
         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;
@@ -774,17 +774,17 @@
     q33_can.id = 0x3; 
 
     //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);
+    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, q2_control.a1);
-    pack_cmd(&q22_can, q2_control.a2);
-    pack_cmd(&q23_can, q2_control.a3);
+    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, q3_control.a1);
-    pack_cmd(&q32_can, q3_control.a2);
-    pack_cmd(&q33_can, q3_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();