Diff: main.cpp

Revision:

2:32f69175c78e

Parent:

1:aa253b5f5b65

Child:

3:d5d603c7e5b6

--- a/main.cpp	Thu Mar 25 19:32:21 2021 +0000
+++ b/main.cpp	Tue Mar 30 14:11:55 2021 +0000
@@ -341,6 +341,71 @@
     return t;
 }
 
+
+
+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]);
+    
+    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 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 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 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 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 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 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 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]);
+    
+}
+
+
+
+
+
 void spi_isr(void)
 {
     //pc.printf("CS ACTIVE\n");
@@ -356,8 +421,8 @@
                 SPI1->DR = tx_buff[bytecount];
             }
         }
-
     }
+    //pc.printf("RECIEVED: %d BYTES\n", bytecount);
     
     //pc.printf("HERE'S A SPI COMMAND:\n");
     // after reading, save into spi_command
@@ -366,13 +431,18 @@
     for(int i = 0; i < CMD_LEN; i++)
     {
         ((uint16_t*)(&spi_command))[i] = rx_buff[i];
-        //pc.printf("BIT %d RECIEVED\n", i);
+        //pc.printf("WORD %d RECIEVED: %d\n", i, rx_buff[i]);
     }
+    print_SPI_command();
     
     // run control, which fills in tx_buff for the next iteration
+    /*
     if(calc_checksum != spi_command.checksum){
         spi_data.flags[1] = 0xdead;
-        pc.printf("FAILED CHECKSUM\n");}
+        pc.printf("FAILED CHECKSUM\n");
+        pc.printf("ACTUAL: %d\n", calc_checksum);
+        pc.printf("CURRENT: %d\n", spi_command.checksum);}
+    */
         
     //test_control();
     //spi_data.q_abad[0] = 12.0f;
@@ -621,7 +691,6 @@
     cs.fall(&spi_isr);
     pc.printf("done\n\r");
 }
-
     
 int main() {
     wait(1);
@@ -715,11 +784,13 @@
         wait_us(10);
         
         //print heatbeat (always will print message 0)
+        /*
         if (counter3 == 100000) {                                                 //for debugging purposes
             pc.printf("HEARTBEAT\n");
             counter3 = 0;
         }
         counter3++;
-
+        */
         }
+
 }
\ No newline at end of file