Diff: main.cpp

Revision:

7:2f785cb39f11

Parent:

6:077e37d5c2da

Child:

8:450fbccaf4f6

--- a/main.cpp	Thu Apr 01 22:48:48 2021 +0000
+++ b/main.cpp	Fri Apr 02 17:46:49 2021 +0000
@@ -34,12 +34,12 @@
  #define T_MAX 72.0f
  
  /// Joint Soft Stops ///
- #define A_LIM_P 1.5f
- #define A_LIM_N -1.5f
- #define H_LIM_P 5.0f
- #define H_LIM_N -5.0f
- #define K_LIM_P 0.2f
- #define K_LIM_N 7.7f
+ #define A1_LIM_P 1.5f
+ #define A1_LIM_N -1.5f
+ #define A2_LIM_P 1.5f
+ #define A2_LIM_N -1.5f
+ #define A3_LIM_P 1.5f
+ #define A3_LIM_N -1.5f
  #define KP_SOFTSTOP 100.0f
  #define KD_SOFTSTOP 0.4f;
 
@@ -654,19 +654,19 @@
         g3_control.a3.kd = spi_command.kd_3s[2];
         g3_control.a3.t_ff = spi_command.tau_3s_ff[2];
         
-        //SPI FLAGS RETURN                                                                      //MIGHT NEED FIXING LATER WHEN WE DO RETURN DATA
+        //SPI FLAGS RETURN                                                                      //IMPLEMENTS THE JOINT SOFT STOP RIGHT HERE
         spi_data.flags[0] = 0;
         spi_data.flags[1] = 0;
         spi_data.flags[2] = 0;
-        spi_data.flags[0] |= softstop_joint(g1_state.a1, &g1_control.a1, A_LIM_P, A_LIM_N);
-        spi_data.flags[0] |= (softstop_joint(g1_state.a2, &g1_control.a2, H_LIM_P, H_LIM_N))<<1;
-        //spi_data.flags[0] |= (softstop_joint(g1_state.k, &g1_control.k, K_LIM_P, K_LIM_N))<<2;
-        spi_data.flags[1] |= softstop_joint(g2_state.a1, &g2_control.a1, A_LIM_P, A_LIM_N);
-        spi_data.flags[1] |= (softstop_joint(g2_state.a2, &g2_control.a2, H_LIM_P, H_LIM_N))<<1;
-        //spi_data.flags[1] |= (softstop_joint(g2_state.k, &q2_control.k, K_LIM_P, K_LIM_N))<<2;
-        spi_data.flags[2] |= softstop_joint(g3_state.a1, &g3_control.a1, A_LIM_P, A_LIM_N);
-        spi_data.flags[2] |= (softstop_joint(g3_state.a2, &g3_control.a2, H_LIM_P, H_LIM_N))<<1;
-        //spi_data.flags[2] |= (softstop_joint(g3_state.k, &q3_control.k, K_LIM_P, K_LIM_N))<<2;
+        spi_data.flags[0] |= softstop_joint(g1_state.a1, &g1_control.a1, A1_LIM_P, A1_LIM_N);
+        spi_data.flags[0] |= (softstop_joint(g1_state.a2, &g1_control.a2, A2_LIM_P, A2_LIM_N))<<1;
+        spi_data.flags[0] |= (softstop_joint(g1_state.a3, &g1_control.a3, A3_LIM_P, A3_LIM_N))<<2;
+        spi_data.flags[1] |= softstop_joint(g2_state.a1, &g2_control.a1, A1_LIM_P, A1_LIM_N);
+        spi_data.flags[1] |= (softstop_joint(g2_state.a2, &g2_control.a2, A2_LIM_P, A2_LIM_N))<<1;
+        spi_data.flags[1] |= (softstop_joint(g2_state.a3, &g2_control.a3, A3_LIM_P, A3_LIM_N))<<2;
+        spi_data.flags[2] |= softstop_joint(g3_state.a1, &g3_control.a1, A1_LIM_P, A1_LIM_N);
+        spi_data.flags[2] |= (softstop_joint(g3_state.a2, &g3_control.a2, A2_LIM_P, A2_LIM_N))<<1;
+        spi_data.flags[2] |= (softstop_joint(g3_state.a3, &g3_control.a3, A3_LIM_P, A3_LIM_N))<<2;
         
         //spi_data.flags[0] = 0xbeef;
         //spi_data.flags[1] = 0xbeef;