Diff: main.cpp

Revision:

52:8fff6f1a3f50

Parent:

49:8dcdb8a89d0e

Child:

53:89565c1d9115

--- a/main.cpp	Tue Oct 26 08:56:41 2021 +0000
+++ b/main.cpp	Mon Dec 20 01:24:06 2021 +0000
@@ -4,10 +4,12 @@
 /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling
 
 #define REST_MODE 0
-#define CALIBRATION_MODE 1
+#define CALIBRATION_MODE 1./
 #define MOTOR_MODE 2
 #define SETUP_MODE 4
 #define ENCODER_MODE 5
+#define SPEED_MODE 6
+#define Position_MODE 7
 
 #define VERSION_NUM "1.6"
 
@@ -41,7 +43,7 @@
 Serial pc(PA_2, PA_3);
 
 
-CAN          can(PB_8, PB_9, 1000000);      // CAN Rx pin name, CAN Tx pin name, 1000kbps
+CAN          can(PB_8, PB_9, 800000);      // CAN Rx pin name, CAN Tx pin name, 1000kbps
 CANMessage   rxMsg;
 CANMessage   txMsg;
 
@@ -68,12 +70,32 @@
             state_change = 1;
             gpio.led->write(0); 
             }
+//************WYC ADD **********2021.11.04**************//
+        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]==0xFA))){
+            state = SPEED_MODE;
+            state_change = 1;
+            gpio.led->write(1);
+            }
+//************WYC ADD **********2021.11.04**************//
+//************YZ ADD **********2021.11.05**************//
+        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 = Position_MODE;
+            state_change = 1;
+            gpio.led->write(1);
+            }
+//************YZ ADD **********2021.11.05**************//
         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(state == MOTOR_MODE){
             unpack_cmd(rxMsg, &controller);
             }
+*/
+        else if(state == MOTOR_MODE ||state == SPEED_MODE||state == Position_MODE)
+            { //WYC ADD 2021.11.04
+            unpack_cmd(rxMsg, &controller);
+            }
         pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
         can.write(txMsg);
         }
@@ -132,6 +154,34 @@
     state_change = 0;
     printf("\n\r Entering Motor Mode \n\r");
     }
+
+//****************WYC ADD ************2021.11.04**********//
+    void enter_speed_mode(void){
+    controller.ovp_flag = 0;
+    gpio.enable->write(1);                                                      // Enable gate drive
+    reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
+    wait(.001);
+    controller.i_d_ref = 0;
+    controller.i_q_ref = 0;                                                     // Current Setpoints
+    gpio.led->write(1);                                                     // Turn on status LED
+    state_change = 0;
+    printf("\n\r Entering SPEED Mode \n\r");
+    }
+//****************WYC ADD ************2021.11.04**********//
+
+//****************YZ ADD ************2021.11.05**********//
+    void enter_Position_mode(void){
+    controller.ovp_flag = 0;
+    gpio.enable->write(1);                                                      // Enable gate drive
+    reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
+    wait(.001);
+    controller.i_d_ref = 0;
+    controller.i_q_ref = 0;                                                     // Current Setpoints
+    gpio.led->write(1);                                                     // Turn on status LED
+    state_change = 0;
+    printf("\n\r Entering Position Mode \n\r");
+    }
+//****************YZ ADD ************2021.11.05**********//
     
 void calibrate(void){
     gpio.enable->write(1);                                                      // Enable gate drive
@@ -222,6 +272,45 @@
             
                 }     
                 break;
+///     wyc////        
+            case SPEED_MODE:                                                   // Run SPEED control WYC 2021.11.04
+                if(state_change){
+                    enter_speed_mode();
+                    }
+                else{
+                    velocity_control(&controller);
+                    if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
+                        controller.i_d_ref = 0;
+                        controller.i_q_ref = 0;
+                        controller.kp = 0;
+                        controller.kd = 0;
+                        controller.t_ff = 0;
+                    } 
+                    commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
+                    controller.timeout += 1;
+                }
+                break;
+   ////wyc////////     
+
+  ///     YZ////        
+            case Position_MODE:                                                   // Run Position control WYC 2021.11.05
+                if(state_change){
+                    enter_Position_mode();
+                    }
+                else{
+                    Position_control(&controller);
+                    if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
+                        controller.i_d_ref = 0;
+                        controller.i_q_ref = 0;
+                        controller.kp = 0;
+                        controller.kd = 0;
+                        controller.t_ff = 0;
+                    } 
+                    commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
+                    controller.timeout += 1;
+                }
+                break;
+   ////YZ////////      
             case SETUP_MODE:
                 if(state_change){
                     enter_setup_state();