Aditya Mehrotra / SPINE_FIDDY_TEST

test code for SPI communications with any board

Dependencies:   mbed-dev

Diff: main.cpp

Revision:
13:ad3ca70bf929
Parent:
12:b203f3ae57d0
Child:
14:8a7fd1ebc209
--- a/main.cpp	Wed May 12 15:11:14 2021 +0000
+++ b/main.cpp	Tue Jul 20 19:34:16 2021 +0000
@@ -29,9 +29,9 @@
  #define KP_MIN 0.0f
  #define KP_MAX 500.0f
  #define KD_MIN 0.0f
- #define KD_MAX 10.0f
- #define T_MIN -72.0f
- #define T_MAX 72.0f
+ #define KD_MAX 5.0f
+ #define T_MIN -18.0f
+ #define T_MAX 18.0f
  
  /// Joint Soft Stops ///
  #define A1_LIM_P 1.5f
@@ -370,27 +370,26 @@
 }
 
 
-/*
+
 void print_SPI_command() {
     pc.printf("SPI MESSAGE RECIEVED:\n");
-    //CAN ONE
     pc.printf("MOTOR 1-1 Q: %f\n", spi_command.q_des_1s[0]);
     pc.printf("MOTOR 1-1 Qd: %f\n", spi_command.qd_des_1s[0]);
     pc.printf("MOTOR 1-1 Kp: %f\n", spi_command.kp_1s[0]);
     pc.printf("MOTOR 1-1 Kd: %f\n", spi_command.kd_1s[0]);
     pc.printf("MOTOR 1-1 T_FF: %f\n", spi_command.tau_1s_ff[0]);
     
-    pc.printf("MOTOR 1-2 Q: %f\n", spi_command.q_des_2s[0]);
-    pc.printf("MOTOR 1-2 Qd: %f\n", spi_command.qd_des_2s[0]);
-    pc.printf("MOTOR 1-2 Kp: %f\n", spi_command.kp_2s[0]);
-    pc.printf("MOTOR 1-2 Kd: %f\n", spi_command.kd_2s[0]);
-    pc.printf("MOTOR 1-2 T_FF: %f\n", spi_command.tau_2s_ff[0]);
+    //pc.printf("MOTOR 1-2 Q: %f\n", spi_command.q_des_2s[0]);
+    //pc.printf("MOTOR 1-2 Qd: %f\n", spi_command.qd_des_2s[0]);
+    //pc.printf("MOTOR 1-2 Kp: %f\n", spi_command.kp_2s[0]);
+    //pc.printf("MOTOR 1-2 Kd: %f\n", spi_command.kd_2s[0]);
+    //pc.printf("MOTOR 1-2 T_FF: %f\n", spi_command.tau_2s_ff[0]);
     
-    pc.printf("MOTOR 1-3 Q: %f\n", spi_command.q_des_3s[0]);
-    pc.printf("MOTOR 1-3 Qd: %f\n", spi_command.qd_des_3s[0]);
-    pc.printf("MOTOR 1-3 Kp: %f\n", spi_command.kp_3s[0]);
-    pc.printf("MOTOR 1-3 Kd: %f\n", spi_command.kd_3s[0]);
-    pc.printf("MOTOR 1-3 T_FF: %f\n", spi_command.tau_3s_ff[0]);
+    //pc.printf("MOTOR 1-3 Q: %f\n", spi_command.q_des_3s[0]);
+    //pc.printf("MOTOR 1-3 Qd: %f\n", spi_command.qd_des_3s[0]);
+    //pc.printf("MOTOR 1-3 Kp: %f\n", spi_command.kp_3s[0]);
+    //pc.printf("MOTOR 1-3 Kd: %f\n", spi_command.kd_3s[0]);
+    //pc.printf("MOTOR 1-3 T_FF: %f\n", spi_command.tau_3s_ff[0]);
     
     pc.printf("MOTOR 2-1 Q: %f\n", spi_command.q_des_1s[1]);
     pc.printf("MOTOR 2-1 Qd: %f\n", spi_command.qd_des_1s[1]);
@@ -398,38 +397,37 @@
     pc.printf("MOTOR 2-1 Kd: %f\n", spi_command.kd_1s[1]);
     pc.printf("MOTOR 2-1_FF: %f\n", spi_command.tau_1s_ff[1]);
     
-    pc.printf("MOTOR 2-2 Q: %f\n", spi_command.q_des_2s[1]);
-    pc.printf("MOTOR 2-2 Qd: %f\n", spi_command.qd_des_2s[1]);
-    pc.printf("MOTOR 2-2 Kp: %f\n", spi_command.kp_2s[1]);
-    pc.printf("MOTOR 2-2 Kd: %f\n", spi_command.kd_2s[1]);
-    pc.printf("MOTOR 2-2 T_FF: %f\n", spi_command.tau_2s_ff[1]);
+    //pc.printf("MOTOR 2-2 Q: %f\n", spi_command.q_des_2s[1]);
+    //pc.printf("MOTOR 2-2 Qd: %f\n", spi_command.qd_des_2s[1]);
+    //pc.printf("MOTOR 2-2 Kp: %f\n", spi_command.kp_2s[1]);
+    //pc.printf("MOTOR 2-2 Kd: %f\n", spi_command.kd_2s[1]);
+    //pc.printf("MOTOR 2-2 T_FF: %f\n", spi_command.tau_2s_ff[1]);
     
-    pc.printf("MOTOR 2-3 Q: %f\n", spi_command.q_des_3s[1]);
-    pc.printf("MOTOR 2-3 Qd: %f\n", spi_command.qd_des_3s[1]);
-    pc.printf("MOTOR 2-3 Kp: %f\n", spi_command.kp_3s[1]);
-    pc.printf("MOTOR 2-3 Kd: %f\n", spi_command.kd_3s[1]);
-    pc.printf("MOTOR 2-3 T_FF: %f\n", spi_command.tau_3s_ff[1]);
+    //pc.printf("MOTOR 2-3 Q: %f\n", spi_command.q_des_3s[1]);
+    //pc.printf("MOTOR 2-3 Qd: %f\n", spi_command.qd_des_3s[1]);
+    //pc.printf("MOTOR 2-3 Kp: %f\n", spi_command.kp_3s[1]);
+    //pc.printf("MOTOR 2-3 Kd: %f\n", spi_command.kd_3s[1]);
+    //pc.printf("MOTOR 2-3 T_FF: %f\n", spi_command.tau_3s_ff[1]);
     
-    pc.printf("MOTOR 3-1 Q: %f\n", spi_command.q_des_1s[2]);
-    pc.printf("MOTOR 3-1 Qd: %f\n", spi_command.qd_des_1s[2]);
-    pc.printf("MOTOR 3-1 Kp: %f\n", spi_command.kp_1s[2]);
-    pc.printf("MOTOR 3-1 Kd: %f\n", spi_command.kd_1s[2]);
-    pc.printf("MOTOR 3-1 T_FF: %f\n", spi_command.tau_1s_ff[2]);
+    //pc.printf("MOTOR 3-1 Q: %f\n", spi_command.q_des_1s[2]);
+    //pc.printf("MOTOR 3-1 Qd: %f\n", spi_command.qd_des_1s[2]);
+    //pc.printf("MOTOR 3-1 Kp: %f\n", spi_command.kp_1s[2]);
+    //pc.printf("MOTOR 3-1 Kd: %f\n", spi_command.kd_1s[2]);
+    //pc.printf("MOTOR 3-1 T_FF: %f\n", spi_command.tau_1s_ff[2]);
     
-    pc.printf("MOTOR 3-2 Q: %f\n", spi_command.q_des_2s[2]);
-    pc.printf("MOTOR 3-2 Qd: %f\n", spi_command.qd_des_2s[2]);
-    pc.printf("MOTOR 3-2 Kp: %f\n", spi_command.kp_2s[2]);
-    pc.printf("MOTOR 3-2 Kd: %f\n", spi_command.kd_2s[2]);
-    pc.printf("MOTOR 3-2 T_FF: %f\n", spi_command.tau_2s_ff[2]);
+    //pc.printf("MOTOR 3-2 Q: %f\n", spi_command.q_des_2s[2]);
+    //pc.printf("MOTOR 3-2 Qd: %f\n", spi_command.qd_des_2s[2]);
+    //pc.printf("MOTOR 3-2 Kp: %f\n", spi_command.kp_2s[2]);
+    //pc.printf("MOTOR 3-2 Kd: %f\n", spi_command.kd_2s[2]);
+    //pc.printf("MOTOR 3-2 T_FF: %f\n", spi_command.tau_2s_ff[2]);
     
-    pc.printf("MOTOR 3-3 Q: %f\n", spi_command.q_des_3s[2]);
-    pc.printf("MOTOR 3-3 Qd: %f\n", spi_command.qd_des_3s[2]);
-    pc.printf("MOTOR 3-3 Kp: %f\n", spi_command.kp_3s[2]);
-    pc.printf("MOTOR 3-3 Kd: %f\n", spi_command.kd_3s[2]);
-    pc.printf("MOTOR 3-3 T_FF: %f\n", spi_command.tau_3s_ff[2]);
+    //pc.printf("MOTOR 3-3 Q: %f\n", spi_command.q_des_3s[2]);
+    //pc.printf("MOTOR 3-3 Qd: %f\n", spi_command.qd_des_3s[2]);
+    //pc.printf("MOTOR 3-3 Kp: %f\n", spi_command.kp_3s[2]);
+    //pc.printf("MOTOR 3-3 Kd: %f\n", spi_command.kd_3s[2]);
+    //pc.printf("MOTOR 3-3 T_FF: %f\n", spi_command.tau_3s_ff[2]);
     
 }
-*/
 
 
 /*
@@ -768,7 +766,7 @@
 void init_spi(void){
     SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
     spi->format(16, 0);
-    spi->frequency(6000000);
+    spi->frequency(12000000);
     spi->reply(0x0);
     cs.fall(&spi_isr);
     pc.printf("done\n\r");
@@ -791,7 +789,7 @@
     //can2.frequency(1000000);                     // set bit rate to 1Mbps
     //can2.attach(&rxISR2);                 // attach 'CAN receive-complete' interrupt handler
     can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
-    can3.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
+    //can3.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
 
     memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
     memset(&spi_data, 0, sizeof(spi_data_t));
@@ -872,18 +870,7 @@
         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)
-        /*
-        if (counter3 == 100000) {                                                 //for debugging purposes
-            pc.printf("HEARTBEAT\n");
-            counter3 = 0;
-        }
-        counter3++;
-        */
-        }
+        ///wait(0.01);
+    }
 
 }
\ No newline at end of file