eeprom_test

Dependencies:   mbed FastPWM

Revision:
49:e7bcfc244d40
Parent:
48:889798ff9329
Child:
50:3c630b5eba9f
--- a/main.cpp	Wed Jan 29 13:50:24 2020 +0000
+++ b/main.cpp	Wed Feb 05 08:35:58 2020 +0000
@@ -75,6 +75,7 @@
 
 extern int CID_RX_CMD;
 extern int CID_RX_REF_POSITION;
+extern int CID_RX_REF_VALVE_POS;
 extern int CID_RX_REF_PWM;
 
 extern int CID_TX_INFO;
@@ -265,7 +266,6 @@
     }
 
     Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER);
-//    Ref_Valve_Pos_FF = 0.0;
     return Ref_Valve_Pos_FF;
     
 }
@@ -330,8 +330,10 @@
             break;
         }
     }
-
-    V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
+    if(vel.sen>=0.0f)
+        V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
+    else
+        V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; // - (float) FLOWFORCE_GAIN * 0.01f * vel.sen / 30.0f;
     //V_out = VALVE_PWM_RAW_FB;
 }
 
@@ -928,10 +930,10 @@
                 }
                 if (need_enc_init) {
                     if (TMR3_COUNT_DEADZONE < (int) (0.5f * (float) TMR_FREQ_5k)) {
-                        V_out = VALVE_VOLTAGE_LIMIT;
+                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_plus_end = pos.sen;
                     } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) {
-                        V_out = -VALVE_VOLTAGE_LIMIT;
+                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_minus_end = pos.sen;
                     } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false;
                     temp_time = TMR_FREQ_5k;
@@ -1104,16 +1106,16 @@
                 if (TMR3_COUNT_FLOWRATE == 0) {
                     if (pos_plus_end == pos_minus_end) need_enc_init = true;
                     else {
-                        V_out = -VALVE_VOLTAGE_LIMIT;
+                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                         temp_time = (int) (0.5f * (float) TMR_FREQ_5k);
                     }
                 }
                 if (need_enc_init) {
                     if (TMR3_COUNT_FLOWRATE < (int) (0.5f * (float) TMR_FREQ_5k)) {
-                        V_out = VALVE_VOLTAGE_LIMIT;
+                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_plus_end = pos.sen;
                     } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) {
-                        V_out = -VALVE_VOLTAGE_LIMIT;
+                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_minus_end = pos.sen;
                     } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) {
                         need_enc_init = false;
@@ -1216,12 +1218,12 @@
 
             case MODE_PRESSURE_SENSOR_CALIB: {
                 if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
-                    V_out = -VALVE_VOLTAGE_LIMIT;
+                    V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                     if (TMR3_COUNT_PRES_CALIB >= TMR_FREQ_5k) {
                         CUR_PRES_A_sum += CUR_PRES_A;
                     }
                 } else if (TMR3_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) {
-                    V_out = VALVE_VOLTAGE_LIMIT;
+                    V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
                     if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) {
                         CUR_PRES_B_sum += CUR_PRES_B;
                     }
@@ -1250,8 +1252,8 @@
             case MODE_ROTARY_FRICTION_TUNING: {
                 if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4.0f + 3.0f * sin(2 * 3.14159f * 0.5f * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f * 0.05f);
                 V_out = PWM_out * sin(2 * 3.14159f * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f);
-                if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT;
-                else V_out = -VALVE_VOLTAGE_LIMIT;
+                if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
+                else V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; 
                 TMR3_COUNT_ROTARY_FRIC_TUNE++;
                 if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
                     TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
@@ -1307,23 +1309,23 @@
                 VALVE_DZ_timer = VALVE_DZ_timer + 1;
                 if(first_check == 0) {
                     if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        V_out = VALVE_VOLTAGE_LIMIT;
+                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
                     } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        V_out = VALVE_VOLTAGE_LIMIT;
+                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_plus_end = pos.sen;
                     } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
-                        V_out = -VALVE_VOLTAGE_LIMIT;
+                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                     } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
-                        V_out = -VALVE_VOLTAGE_LIMIT;
+                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_minus_end = pos.sen;
                     } else if(VALVE_DZ_timer < (int) (3.0f * (float) TMR_FREQ_5k)) {
-                        V_out = (float) P_GAIN_JOINT_POSITION * 0.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                        V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                     } else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) {
-                        V_out = (float) P_GAIN_JOINT_POSITION * 0.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                        V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                         data_num = data_num + 1;
                         VALVE_POS_TMP = VALVE_POS_TMP + value;
                     } else if(VALVE_DZ_timer == (int) (4.0f * (float) TMR_FREQ_5k)) {
-                        V_out = (float) P_GAIN_JOINT_POSITION * 0.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                        V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                         DDV_POS_AVG = VALVE_POS_TMP / data_num;
                         START_POS = pos.sen;
                         VALVE_POS_TMP = 0;
@@ -1353,6 +1355,8 @@
                             DZ_case = 0;
                         }
 
+                        CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6));
+                        
                         VEL_POINT = 0;
                         first_check = 1;
                         DZ_DIRECTION = 1;
@@ -1365,7 +1369,7 @@
                 } else {
                     if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) {
                         if(VALVE_DZ_timer < (int) (1.0 * (float) TMR_FREQ_5k)) {
-                            V_out = (float) P_GAIN_JOINT_POSITION * 0.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                            V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                         } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
                             START_POS = pos.sen;
                         } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
@@ -1406,7 +1410,7 @@
                         }
                     } else if((DZ_case == -1 && DZ_NUM == 2) | (DZ_case == 1 && DZ_NUM == 2)) {
                         if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                            V_out = (float) P_GAIN_JOINT_POSITION * 0.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                            V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                         } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
                             START_POS = pos.sen;
                         } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
@@ -1453,7 +1457,7 @@
                         }
                     } else if(DZ_case == 0 && DZ_NUM ==1) {
                         if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                            V_out = (float) P_GAIN_JOINT_POSITION * 0.01f* (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                            V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                         } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
                             START_POS = pos.sen;
                         } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
@@ -1493,7 +1497,7 @@
                         }
                     } else {
                         if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                            V_out = (float) P_GAIN_JOINT_POSITION * 0.01f* (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                            V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                         } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
                             START_POS = pos.sen;
                         } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
@@ -1543,17 +1547,17 @@
                 VALVE_FR_timer = VALVE_FR_timer + 1;
                 if(first_check == 0) {
                     if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        V_out = VALVE_VOLTAGE_LIMIT;
+                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
                         //CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
                     } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        V_out = VALVE_VOLTAGE_LIMIT;
+                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_plus_end = pos.sen;
                         //                    CAN_TX_PRES((int16_t) (V_out), (int16_t) (7));
                     } else if(VALVE_FR_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
-                        V_out = -VALVE_VOLTAGE_LIMIT;
+                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                     } else if(VALVE_FR_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
                         //                    CAN_TX_PRES((int16_t) (V_out), (int16_t) (8));
-                        V_out = -VALVE_VOLTAGE_LIMIT;
+                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
                         pos_minus_end = pos.sen;
                         first_check = 1;
                         VALVE_FR_timer = 0;
@@ -1564,7 +1568,7 @@
                     }
                 } else {
                     if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        V_out = (float) P_GAIN_JOINT_POSITION * 0.01f* (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+                        V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
                     } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
                         data_num = 0;
                         valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_CENTER;
@@ -1607,6 +1611,7 @@
                         first_check = 0;
                         VALVE_FR_timer = 0;
                         CONTROL_MODE = MODE_NO_ACT;
+//                        CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
                     }
                 }
                 break;
@@ -1645,12 +1650,12 @@
         /*******************************************************
         ***     PWM
         ********************************************************/
-        if (V_out >= 12000.0f) {
-            V_out = 12000.0f;
-        } else if(V_out<=-12000.0f) {
-            V_out = -12000.0f;
+        if (V_out >= VALVE_VOLTAGE_LIMIT*1000.0f) {
+            V_out = VALVE_VOLTAGE_LIMIT*1000.0f;
+        } else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) {
+            V_out = -VALVE_VOLTAGE_LIMIT*1000.0f;
         }
-        PWM_out= V_out/SUPPLY_VOLTAGE; // Full duty : 12000.0mV
+        PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f); // Full duty : 12000.0mV
 
         // Saturation of output voltage to 5.0V
 //        if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
@@ -1690,7 +1695,7 @@
         if (flag_data_request[0] == HIGH) {
             //position+velocity
             //CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
-            CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f));
+            CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (V_out/100.0f), (int16_t) (torq.sen*10.0f));
         }
 
         if (flag_data_request[1] == HIGH) {
@@ -1708,7 +1713,7 @@
 
         if (flag_data_request[2] == HIGH) {
             //pressure A and B
-            CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF * 1000.0)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+            CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (PRES_A_VREF * 1000.0f)); // CUR_PRES_X : 0(0bar)~4096(210bar)
             //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar)
             //                        CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.));
             //            CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.);