Diff: main.cpp

Revision:

12:5f97bcab6c50

Parent:

11:40d5d9b5d2ba

Child:

13:87e7ce883f6a

--- a/main.cpp	Wed May 26 18:14:09 2021 +0000
+++ b/main.cpp	Thu Feb 03 19:10:54 2022 +0000
@@ -459,6 +459,48 @@
 
 
 
+void process()
+{
+    // 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); //NOTE, CHECK THIS WE WANT TO DO IT ON THE FIRST 16 SPI BYTES
+    for(int i = 0; i < DATA_LEN; i++){
+        tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
+}
+
 
 
 void spi_isr(void)
@@ -482,7 +524,7 @@
     //pc.printf("HERE'S A SPI COMMAND:\n");
     // after reading, save into spi_command
     // should probably check checksum first!
-    uint32_t calc_checksum = xor_checksum((uint32_t*)rx_buff,32);
+    uint32_t calc_checksum = xor_checksum((uint32_t*)rx_buff,48);
     for(int i = 0; i < CMD_LEN; i++)
     {
         ((uint16_t*)(&spi_command))[i] = rx_buff[i];
@@ -501,9 +543,10 @@
     */
 
     //print_SPI_data();
-    control();
-    PackAll();
-    WriteAll();
+    process();
+    //control();
+    //PackAll();
+    //WriteAll();
 
 
     //for (int i = 0; i<TX_LEN; i++) {
@@ -860,12 +903,12 @@
     while(1) {
         //pc.printf("test, of SPINE\r\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);
+        //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)