Diff: main.cpp

Revision:

6:52b418e1d6d0

Parent:

3:9ef9b4c66648

--- a/main.cpp	Wed May 02 18:19:22 2018 +0000
+++ b/main.cpp	Sun Jul 15 19:00:15 2018 +0000
@@ -53,8 +53,8 @@
 
 
 Serial       pc(PA_2, PA_3);
-CAN          can1(PB_12, PB_13);  // CAN Rx pin name, CAN Tx pin name
-CAN          can2(PB_8, PB_9);  // CAN Rx pin name, CAN Tx pin name
+CAN          can1(PB_12, PB_13, 1000000);  // CAN Rx pin name, CAN Tx pin name
+CAN          can2(PB_8, PB_9, 1000000);  // CAN Rx pin name, CAN Tx pin name
 
 CANMessage   rxMsg1, rxMsg2;
 CANMessage   txMsg1, txMsg2;
@@ -340,8 +340,11 @@
     }
     
     // run control, which fills in tx_buff for the next iteration
-    if(calc_checksum != spi_command.checksum){
-        spi_data.flags[1] = 0xdead;}
+    if(calc_checksum != spi_command.checksum)
+    {
+        spi_data.flags[1] = 0xdead;
+        //printf("Bad Checksum!\n\r");
+        }
         
     //test_control();
     //spi_data.q_abad[0] = 12.0f;
@@ -449,51 +452,52 @@
         
         memset(&l1_control, 0, sizeof(l1_control));
         memset(&l2_control, 0, sizeof(l2_control));
+        float tmax = 0.5f;
         
-        l1_control.a.p_des = spi_command.q_des_abad[0];
-        l1_control.a.v_des  = spi_command.qd_des_abad[0];
-        l1_control.a.kp = spi_command.kp_abad[0];
-        l1_control.a.kd = spi_command.kd_abad[0];
-        l1_control.a.t_ff = spi_command.tau_abad_ff[0];
+        l1_control.a.p_des = 0;//spi_command.q_des_abad[0];
+        l1_control.a.v_des  = 0;//spi_command.qd_des_abad[0];
+        l1_control.a.kp = 0;//spi_command.kp_abad[0];
+        l1_control.a.kd = 0;//spi_command.kd_abad[0];
+        l1_control.a.t_ff = fmaxf(fminf(spi_command.tau_abad_ff[0], tmax),-tmax);
         
-        l1_control.h.p_des = spi_command.q_des_hip[0];
-        l1_control.h.v_des  = spi_command.qd_des_hip[0];
-        l1_control.h.kp = spi_command.kp_hip[0];
-        l1_control.h.kd = spi_command.kd_hip[0];
-        l1_control.h.t_ff = spi_command.tau_hip_ff[0];
+        l1_control.h.p_des = 0;//spi_command.q_des_hip[0];
+        l1_control.h.v_des  = 0;//spi_command.qd_des_hip[0];
+        l1_control.h.kp = 0;//spi_command.kp_hip[0];
+        l1_control.h.kd = 0;//spi_command.kd_hip[0];
+        l1_control.h.t_ff = fmaxf(fminf(spi_command.tau_hip_ff[0], tmax), -tmax);
         
-        l1_control.k.p_des = spi_command.q_des_knee[0];
-        l1_control.k.v_des  = spi_command.qd_des_knee[0];
-        l1_control.k.kp = spi_command.kp_knee[0];
-        l1_control.k.kd = spi_command.kd_knee[0];
-        l1_control.k.t_ff = spi_command.tau_knee_ff[0];
+        l1_control.k.p_des = 0;//spi_command.q_des_knee[0];
+        l1_control.k.v_des  = 0;//spi_command.qd_des_knee[0];
+        l1_control.k.kp = 0;//spi_command.kp_knee[0];
+        l1_control.k.kd = 0;//spi_command.kd_knee[0];
+        l1_control.k.t_ff = fmaxf(fminf(spi_command.tau_knee_ff[0], tmax), -tmax);
         
-        l2_control.a.p_des = spi_command.q_des_abad[1];
-        l2_control.a.v_des  = spi_command.qd_des_abad[1];
-        l2_control.a.kp = spi_command.kp_abad[1];
-        l2_control.a.kd = spi_command.kd_abad[1];
-        l2_control.a.t_ff = spi_command.tau_abad_ff[1];
+        l2_control.a.p_des = 0;//spi_command.q_des_abad[1];
+        l2_control.a.v_des  = 0;//spi_command.qd_des_abad[1];
+        l2_control.a.kp = 0;//spi_command.kp_abad[1];
+        l2_control.a.kd = 0;//spi_command.kd_abad[1];
+        l2_control.a.t_ff = fmaxf(fminf(spi_command.tau_abad_ff[1], tmax), -tmax);
         
-        l2_control.h.p_des = spi_command.q_des_hip[1];
-        l2_control.h.v_des  = spi_command.qd_des_hip[1];
-        l2_control.h.kp = spi_command.kp_hip[1];
-        l2_control.h.kd = spi_command.kd_hip[1];
-        l2_control.h.t_ff = spi_command.tau_hip_ff[1];
+        l2_control.h.p_des = 0;//spi_command.q_des_hip[1];
+        l2_control.h.v_des  = 0;//spi_command.qd_des_hip[1];
+        l2_control.h.kp = 0;//spi_command.kp_hip[1];
+        l2_control.h.kd = 0;//spi_command.kd_hip[1];
+        l2_control.h.t_ff = fmaxf(fminf(spi_command.tau_hip_ff[1], tmax), -tmax);
         
-        l2_control.k.p_des = spi_command.q_des_knee[1];
-        l2_control.k.v_des  = spi_command.qd_des_knee[1];
-        l2_control.k.kp = spi_command.kp_knee[1];
-        l2_control.k.kd = spi_command.kd_knee[1];
-        l2_control.k.t_ff = spi_command.tau_knee_ff[1];
+        l2_control.k.p_des = 0;//spi_command.q_des_knee[1];
+        l2_control.k.v_des  = 0;//spi_command.qd_des_knee[1];
+        l2_control.k.kp = 0;//spi_command.kp_knee[1];
+        l2_control.k.kd = 0;//spi_command.kd_knee[1];
+        l2_control.k.t_ff = fmaxf(fminf(spi_command.tau_knee_ff[1], tmax), -tmax);
         
         
         spi_data.flags[0] = 0;
         spi_data.flags[1] = 0;
-        spi_data.flags[0] |= softstop_joint(l1_state.a, &l1_control.a, A_LIM_P, A_LIM_N);
-        spi_data.flags[0] |= (softstop_joint(l1_state.h, &l1_control.h, H_LIM_P, H_LIM_N))<<1;
+        //spi_data.flags[0] |= softstop_joint(l1_state.a, &l1_control.a, A_LIM_P, A_LIM_N);
+        //spi_data.flags[0] |= (softstop_joint(l1_state.h, &l1_control.h, H_LIM_P, H_LIM_N))<<1;
         //spi_data.flags[0] |= (softstop_joint(l1_state.k, &l1_control.k, K_LIM_P, K_LIM_N))<<2;
-        spi_data.flags[1] |= softstop_joint(l2_state.a, &l2_control.a, A_LIM_P, A_LIM_N);
-        spi_data.flags[1] |= (softstop_joint(l2_state.h, &l2_control.h, H_LIM_P, H_LIM_N))<<1;
+        //spi_data.flags[1] |= softstop_joint(l2_state.a, &l2_control.a, A_LIM_P, A_LIM_N);
+        //spi_data.flags[1] |= (softstop_joint(l2_state.h, &l2_control.h, H_LIM_P, H_LIM_N))<<1;
         //spi_data.flags[1] |= (softstop_joint(l2_state.k, &l2_control.k, K_LIM_P, K_LIM_N))<<2;
         
         //spi_data.flags[0] = 0xbeef;
@@ -552,10 +556,10 @@
     //spi.reply(0x0);
     //cs.fall(&spi_isr);
 
-    can1.frequency(1000000);                     // set bit rate to 1Mbps
+    //can1.frequency(1000000);                     // set bit rate to 1Mbps
     //can1.attach(&rxISR1);                 // attach 'CAN receive-complete' interrupt handler
     can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
-    can2.frequency(1000000);                     // set bit rate to 1Mbps
+    //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