Diff: main.cpp

Revision:

13:87e7ce883f6a

Parent:

12:5f97bcab6c50

Child:

14:6fec697f62ef

diff -r 5f97bcab6c50 -r 87e7ce883f6a main.cpp
--- a/main.cpp	Thu Feb 03 19:10:54 2022 +0000
+++ b/main.cpp	Fri Feb 04 19:42:10 2022 +0000
@@ -11,7 +11,7 @@
 
 // length of receive/transmit buffers
 #define RX_LEN 98                                                                   //CHECK THESE BUFFER LENGHTS
-#define TX_LEN 98                                                                   //CHECK THESE BUFFER LENGHTS
+#define TX_LEN 62                                                                   //CHECK THESE BUFFER LENGHTS
 
 // length of outgoing/incoming messages
 #define DATA_LEN 62                                                                 //CHECK THESE BUFFER LENGHTS
@@ -48,6 +48,7 @@
 
 spi_data_t spi_data; // data from spine to up
 spi_command_t spi_command; // data from up to spine
+spi_command_t tmp_crc_chk;
 
 // spi buffers
 uint16_t rx_buff[RX_LEN];
@@ -461,6 +462,7 @@
 
 void process()
 {
+    //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;
@@ -496,7 +498,7 @@
     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
+    spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 30); 
     for(int i = 0; i < DATA_LEN; i++){
         tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
 }
@@ -521,41 +523,26 @@
     }
     //pc.printf("RECIEVED: %d BYTES\n", bytecount);
     
-    //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,48);
-    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();
+    //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);
+   // READ CHECKSUM
+   uint32_t _rx_crc = tmp_crc_chk.checksum;
     
-    // 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("ACTUAL: %d\n", calc_checksum);
-        pc.printf("CURRENT: %d\n", spi_command.checksum);}
-    */
+    if(_crc == _rx_crc) {
+       //pc.printf("CHECKSUM PASSED...");
+       //update crc_chk from buffer
+       spi_command = tmp_crc_chk;
+       //do math on the input
+       process();
+       //control();
+       //new_command = 1;
+    //print_SPI_command();
+    }
+    else{pc.printf("bigger F in the chat...\n");}
 
-    //print_SPI_data();
-    process();
-    //control();
-    //PackAll();
-    //WriteAll();
-
-
-    //for (int i = 0; i<TX_LEN; i++) {
-     //   tx_buff[i] = 2*rx_buff[i];
-    //}
-//    for (int i=0; i<TX_LEN; i++) {
-//        //printf("%d ", rx_buff[i]);
-//    }
-    //printf("\n\r");
+ 
 }
 
 int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){
@@ -903,12 +890,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)