Diff: main.cpp

Revision:

55:6ab83cbbbca7

Parent:

53:e85efce8c1eb

--- a/main.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/main.cpp	Fri Mar 26 05:28:00 2021 +0000
@@ -9,6 +9,7 @@
 #define MOTOR_MODE 2
 #define SETUP_MODE 4
 #define ENCODER_MODE 5
+#define SETPARAM_MODE 6
 
 #define VERSION_NUM "1.9"
 
@@ -76,9 +77,16 @@
         else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE))){
             spi.ZeroPosition();
             }
+                else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFB))){
+                        state = SETPARAM_MODE;
+                        state_change = 1;
+                }
         else if(state == MOTOR_MODE){
             unpack_cmd(rxMsg, &controller);
             }
+                else if(state == SETPARAM_MODE){
+            unpack_param(rxMsg, &controller);
+            }
         pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
         can.write(txMsg);
         }
@@ -217,7 +225,10 @@
                 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
                     controller.i_d_ref = 0;
                     controller.i_q_ref = 0;
-                    controller.kp = 0;
+                    controller.kp_H = 0;
+                                        controller.kp_L = 0;
+                                        controller.kp_M1 = 0;
+                                        controller.kp_M2 = 0;
                     controller.kd = 0;
                     controller.t_ff = 0;
                     } 
@@ -402,6 +413,7 @@
     prefs.load();                                                               // Read flash
     
     can.filter(CAN_ID , 0xFFF, CANStandard, 0);
+        //can.filter(1 , 0xFFF, CANStandard, 1);    // Example, show how to add can dataframe filter
                                                                     
     txMsg.id = CAN_MASTER;
     txMsg.len = 6;