Revision 65:cb3497b549e9, committed 2022-08-19

Comitter:

WinnieLiu

Date:

Fri Aug 19 07:46:05 2022 +0000

Parent:

64:fd695fb9865b

Commit message:

latest code

Changed in this revision

--- a/CAN/CAN_com.cpp	Tue Mar 22 15:02:43 2022 +0000
+++ b/CAN/CAN_com.cpp	Fri Aug 19 07:46:05 2022 +0000
@@ -26,7 +26,7 @@
 /// 2: [velocity[11-4]]
 /// 3: [velocity[3-0], current[11-8]]
 /// 4: [current[7-0]]
-void pack_reply(CANMessage *msg, float p, float v, float t, int version, int calibrate_finish, int state){
+void pack_reply(CANMessage *msg, float p, float v, float t, int version, int calibrate_finish, int state, float iq_ref){
     /*
     // ----- origin -----(p,v,t)
     int p_int = float_to_uint(p, P_MIN, P_MAX, 16);
@@ -45,6 +45,7 @@
     int p_int = float_to_uint(p, P_REPLY_MIN, P_REPLY_MAX, 16);
     int v_int = float_to_uint(v, V_MIN, V_MAX, 12);
     int t_int = float_to_uint(t, T_MIN, T_MAX, 12);
+    int iqref_int = float_to_uint(iq_ref, T_MIN, T_MAX, 12);
     
     msg->data[0] = CAN_ID;
     msg->data[1] = p_int>>8;
@@ -52,6 +53,9 @@
     msg->data[3] = v_int>>4;
     msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8);
     msg->data[5] = t_int&0xFF;
+//    msg->data[6] = iqref_int >> 4;
+//    msg->data[7] = ((iqref_int&0xF)<<4) + version;
+    
     msg->data[6] = (version<<4) + (calibrate_finish&0xF);
     msg->data[7] = state;
     

--- a/CAN/CAN_com.h	Tue Mar 22 15:02:43 2022 +0000
+++ b/CAN/CAN_com.h	Fri Aug 19 07:46:05 2022 +0000
@@ -27,7 +27,8 @@
 #define T_MIN -20.0f
 #define T_MAX 20.0f
 
-void pack_reply(CANMessage *msg, float p, float v, float t, int version, int calibrate_finish, int state);
+//void pack_reply(CANMessage *msg, float p, float v, float t, int version, int calibrate_finish, int state);
+void pack_reply(CANMessage *msg, float p, float v, float t, int version, int calibrate_finish, int state, float iq_ref);
 void unpack_cmd(CANMessage msg, ControllerStruct * controller);
 
 

--- a/FOC/foc.cpp	Tue Mar 22 15:02:43 2022 +0000
+++ b/FOC/foc.cpp	Fri Aug 19 07:46:05 2022 +0000
@@ -234,10 +234,12 @@
     pos = controller->theta_mech;
     modf(pos/(2*PI),&round);
     pos = pos - round*2*PI;
-    if(abs(pos) <= 0.001)
-        pos = abs(pos);
-    else if(pos < 0)
+    if (pos < 0)
         pos = pos + 2*PI;
+//    if(abs(pos) <= 0.001)
+//        pos = abs(pos);
+//    else if(pos < 0)
+//        pos = pos + 2*PI;
     /*-----------------------------------------------*/
     
     /*----- position PID control -----*/

--- a/main.cpp	Tue Mar 22 15:02:43 2022 +0000
+++ b/main.cpp	Fri Aug 19 07:46:05 2022 +0000
@@ -3,16 +3,23 @@
 /// Hardware documentation can be found at build-its.blogspot.com
 /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling
 /// Version for the TI DRV8323 Everything Chip
- 
+
+// ----- CAN and UART -----
 #define REST_MODE 0
-#define MOTOR_MODE 1
-#define HALL_CALIBRATE 2 //hall sensor calibration
-#define SETUP_MODE 3
-#define CALIBRATION_MODE 4
-#define ENCODER_MODE 5
+#define HALL_CALIBRATE 1 //hall sensor calibration
+#define MOTOR_MODE 2
+#define SET_ZERO 3
+
+//----- UART only ------
+#define SETUP_MODE 4
+#define CALIBRATION_MODE 5
+#define ENCODER_MODE 6
  
-#define VERSION_NUM 7
- 
+#define VERSION_NUM 3
+
+#define VER2_0 
+#define PRINT_UART
+
  
 //float __float_reg[64];                                                          // Floats stored in flash
 //float __float_reg[67]; // Floats stored in flash(add three floats: kp, ki, kd)
@@ -108,6 +115,7 @@
         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();
             controller.p_des = 0;
+            state = SET_ZERO;
         }
 //        else if(state == REST_MODE && rxMsg.data[0]==0xFE){
 //            printf("Change Parameters\n\r");
@@ -122,7 +130,7 @@
             /*----- convert theta_mech to 0~359.9999deg -----*/
             hall_presentpos = controller.theta_mech;
             cal_pcmd = controller.theta_mech;
-            float _f_cal_round;
+            static float _f_cal_round;
             modf(cal_pcmd/(2*PI),&_f_cal_round);
             cal_pcmd = cal_pcmd - _f_cal_round*2*PI;
             if(cal_pcmd < 0) cal_pcmd = cal_pcmd + 2*PI;
@@ -133,7 +141,7 @@
         else if(state == MOTOR_MODE){
             unpack_cmd(rxMsg, &controller);
         }
-        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT, VERSION_NUM, calibrate_state, state);
+        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT, VERSION_NUM, calibrate_state, state, controller.i_q_ref);
         can.write(txMsg);
         //can_state = can.write(txMsg);
         //CAN_DEBUG->write(0);
@@ -166,22 +174,27 @@
  
 void enter_menu_state(void){
     drv.disable_gd();
-    gpio.enable->write(0);
-    printf("\n\r\n\r\n\r");
-    printf(" Commands:\n\r");
-    wait_us(10);
-    printf(" m - Motor Mode\n\r");
-    wait_us(10);
-    printf(" c - Calibrate Encoder\n\r");
-    wait_us(10);
-    printf(" s - Setup\n\r");
-    wait_us(10);
-    printf(" e - Display Encoder\n\r");
-    wait_us(10);
-    printf(" z - Set Zero Position\n\r");
-    wait_us(10);
-    printf(" esc - Exit to Menu\n\r");
-    wait_us(10);
+    #ifdef VER2_0
+        gpio.enable->write(0);
+    #endif
+    
+    #ifdef PRINT_UART
+        printf("\n\r\n\r\n\r");
+        printf(" Commands:\n\r");
+        wait_us(10);
+        printf(" m - Motor Mode\n\r");
+        wait_us(10);
+        printf(" c - Calibrate Encoder\n\r");
+        wait_us(10);
+        printf(" s - Setup\n\r");
+        wait_us(10);
+        printf(" e - Display Encoder\n\r");
+        wait_us(10);
+        printf(" z - Set Zero Position\n\r");
+        wait_us(10);
+        printf(" esc - Exit to Menu\n\r");
+        wait_us(10);
+    #endif
     state_change = 0;
     gpio.led->write(0);
 }
@@ -226,8 +239,16 @@
     }
     
 void enter_torque_mode(void){
+    float _f_round, _f_p_des;
+    _f_p_des = controller.theta_mech;
+    modf(_f_p_des/(2*PI),&_f_round);
+    _f_p_des = _f_p_des - _f_round*2*PI;
+    if(_f_p_des < 0) _f_p_des = _f_p_des + 2*PI;
+    controller.p_des = _f_p_des;
     drv.enable_gd();
-    gpio.enable->write(1);
+    #ifdef VER2_0
+        gpio.enable->write(1);
+    #endif
     controller.ovp_flag = 0;
     reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
     wait(.001);
@@ -236,11 +257,13 @@
     gpio.led->write(1);                                                     // Turn on status LED
     state_change = 0;
     printf("\n\r Entering Motor Mode \n\r");
-    }
+}
     
 void calibrate(void){
     drv.enable_gd();
-    gpio.enable->write(1);
+    #ifdef VER2_0
+        gpio.enable->write(1);
+    #endif
     gpio.led->write(1);                                                    // Turn on status LED
     order_phases(&spi, &gpio, &controller, &prefs);                             // Check phase ordering
     calibrate(&spi, &gpio, &controller, &prefs);                                // Perform calibration procedure
@@ -248,7 +271,9 @@
     wait(.2);
     printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
     drv.disable_gd();
-    gpio.enable->write(0);
+    #ifdef VER2_0
+        gpio.enable->write(0);
+    #endif
      state_change = 0;
     }
     
@@ -380,7 +405,9 @@
                         enter_menu_state();
                     else{
                         drv.disable_gd();
-                        gpio.enable->write(0);
+                        #ifdef VER2_0
+                            gpio.enable->write(0);
+                        #endif
                         state_change = 0;
                         gpio.led->write(0);
                     }
@@ -631,7 +658,9 @@
     //drv.enable_gd();
     zero_current(&controller.adc1_offset, &controller.adc2_offset);             // Measure current sensor zero-offset
     drv.disable_gd();
-    gpio.enable->write(0);
+    #ifdef VER2_0
+        gpio.enable->write(0);
+    #endif
  
     wait(.1);
     /*