eeprom_test

Dependencies:   mbed FastPWM

Revision:
12:6f2531038ea4
Parent:
11:82d8768d7351
Child:
13:747daba9cf59
--- a/main.cpp	Fri Aug 23 12:28:08 2019 +0000
+++ b/main.cpp	Tue Aug 27 05:32:44 2019 +0000
@@ -56,10 +56,68 @@
 
 double PWM_out=0.0;
 int ID_index_array[100] = {0};
+
+double CUR_PRES_DIFF_BAR = 0;
+
+double CUR_PRES_A_sum = 0.0;
+double CUR_PRES_B_sum = 0.0;
+double CUR_PRES_A_mean = 0.0;
+double CUR_PRES_B_mean = 0.0;
+double CUR_TORQUE_sum = 0.0;
+double CUR_TORQUE_mean = 0.0;
+double PRES_A_NULL = 200.0;
+double PRES_B_NULL = 200.0;
+double TORQUE_NULL = 3900;
 // =============================================================================
 // =============================================================================
 // =============================================================================
 
+/*******************************************************************************
+ *  REFERENCE MODE
+ ******************************************************************************/
+enum _REFERENCE_MODE{
+    MODE_REF_NO_ACT = 0,                                //0 
+    MODE_REF_DIRECT,                                //1
+    MODE_REF_COS_INC,                                  //2
+    MODE_REF_LINE_INC,                                 //3
+    MODE_REF_SIN_WAVE,                                  //4
+    MODE_REF_SQUARE_WAVE,                                  //5
+};
+
+/*******************************************************************************
+ *  CONTROL MODE
+ ******************************************************************************/
+enum _CONTROL_MODE{
+    //control mode
+    MODE_NO_ACT = 0,                                    //0
+    MODE_VALVE_OPEN_LOOP,                               //1
+    MODE_VALVE_POSITION_CONTROL,                        //2
+    
+    MODE_JOINT_POSITION_TORQUE_CONTROL_PWM,             //3
+    MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION,  //4
+    MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING,        //5
+    
+    MODE_JOINT_POSITION_PRES_CONTROL_PWM,               //6
+    MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION,    //7
+    MODE_VALVE_POSITION_PRES_CONTROL_LEARNING,          //8
+        
+    MODE_TEST_CURRENT_CONTROL,                          //9
+    MODE_TEST_PWM_CONTROL,                              //10
+    
+    //utility
+    MODE_TORQUE_SENSOR_NULLING = 20,                    //20
+    MODE_VALVE_NULLING_AND_DEADZONE_SETTING,            //21
+    MODE_FIND_HOME,                                     //22
+    MODE_VALVE_GAIN_SETTING,                        //23
+    MODE_PRESSURE_SENSOR_NULLING,                       //24
+    MODE_PRESSURE_SENSOR_CALIB,                         //25
+    MODE_ROTARY_FRICTION_TUNING,                        //26
+    
+    MODE_DDV_POS_VS_PWM_ID = 30,                           //30
+    MODE_DDV_DEADZONE_AND_CENTER,                       //31
+    MODE_DDV_POS_VS_FLOWRATE,                           //32
+};
+
 int main()
 {
     /*********************************
@@ -188,6 +246,146 @@
 {
     if ( TIM3->SR & TIM_SR_UIF ) {
         
+        ENC_UPDATE();
+        CUR_PRES_A_BAR = (CUR_PRES_A - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR + 1.;
+        CUR_PRES_B_BAR = (CUR_PRES_B - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR + 1.;
+        //CUR_TORQUE_NM = (CUR_TORQUE - TORQUE_NULL) / (double) TORQUE_SENSOR_PULSE_PER_TORQUE; //
+        CUR_TORQUE_NM = CUR_TORQUE; //
+        CUR_TORQUE_NM_PRESS = 1. * (CUR_PRES_A_BAR - CUR_PRES_B_BAR);
+        
+            // Reference Loop
+        switch (REFERENCE_MODE) {
+            case MODE_REF_NO_ACT:
+            {
+                break;
+            }
+            
+            case MODE_REF_DIRECT:
+            {
+                if (FLAG_REFERENCE_VALVE_PWM) {
+                    Ref_PWM = (double) REF_PWM;
+                }
+                if (FLAG_REFERENCE_VALVE_POSITION) {
+                    Ref_Valve_Pos = (double) REF_VALVE_POSITION;
+                }
+                if (FLAG_REFERENCE_JOINT_POSITION) {
+                    Ref_Joint_Pos = (double) REF_POSITION;
+                    Ref_Joint_Vel = (double) REF_VELOCITY;
+                }
+                if (FLAG_REFERENCE_JOINT_TORQUE) {
+                    Ref_Joint_Torq = (double) REF_TORQUE;
+                }
+                break;
+            }
+            
+            case MODE_REF_COS_INC:
+            {
+                if (FLAG_REFERENCE_VALVE_PWM) {
+                    Ref_PWM = ((double) REF_PWM - (double) INIT_REF_PWM)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_PWM;
+                }
+                if (FLAG_REFERENCE_VALVE_POSITION) {
+                    Ref_Valve_Pos = ((double) REF_VALVE_POSITION - (double) INIT_REF_VALVE_POS)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_VALVE_POS;
+                }
+                if (FLAG_REFERENCE_JOINT_POSITION) {
+                    Ref_Joint_Pos = ((double) REF_POSITION - (double) INIT_REF_POS)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_POS;
+                    Ref_Joint_Vel = 0.0;
+                }
+                if (FLAG_REFERENCE_JOINT_TORQUE) {
+                    Ref_Joint_Torq = ((double) REF_TORQUE - (double) INIT_REF_TORQUE)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_TORQUE;
+                }
+                TMR2_COUNT_REFERENCE++;
+                if (TMR2_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
+                    REFERENCE_MODE = MODE_REF_DIRECT;
+                    TMR2_COUNT_REFERENCE = 0;
+                }
+                break;
+            }
+         
+            
+            case MODE_REF_LINE_INC:
+            {
+                if (FLAG_REFERENCE_VALVE_PWM) {
+                    Ref_PWM = ((double) REF_PWM - (double) INIT_REF_PWM)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_PWM;
+                }
+                if (FLAG_REFERENCE_VALVE_POSITION) {
+                    Ref_Valve_Pos = ((double) REF_VALVE_POSITION - (double) INIT_REF_VALVE_POS)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_VALVE_POS;
+                }
+                if (FLAG_REFERENCE_JOINT_POSITION) {
+                    Ref_Joint_Pos = ((double) REF_POSITION - (double) INIT_REF_POS)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_POS;
+                    Ref_Vel_Test = ((double) REF_POSITION - (double) INIT_REF_POS) / (double) REF_MOVE_TIME_5k * (double) TMR_FREQ_5k;    //   pulse/sec
+                    //Ref_Vel_Test = 10;
+                }
+                if (FLAG_REFERENCE_JOINT_TORQUE) {
+                    Ref_Joint_Torq = ((double) REF_TORQUE - (double) INIT_REF_TORQUE)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_TORQUE;
+                }
+                TMR2_COUNT_REFERENCE++;
+                if (TMR2_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
+                    //REFERENCE_MODE = MODE_REF_DIRECT;
+                    Ref_Vel_Test = 1;
+                    TMR2_COUNT_REFERENCE = REF_MOVE_TIME_5k;
+                    //TMR2_COUNT_REFERENCE = 0;
+                }
+                break;
+            }
+            
+            case MODE_REF_SIN_WAVE:
+            {
+                if (FLAG_REFERENCE_VALVE_PWM) {
+                    Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM;
+                }
+                if (FLAG_REFERENCE_VALVE_POSITION) {
+                    Ref_Valve_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_VALVE_POSITION;
+                }
+                if (FLAG_REFERENCE_JOINT_POSITION) {
+                    Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_POSITION;
+                }
+                if (FLAG_REFERENCE_JOINT_TORQUE) {
+                    Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_TORQUE;
+                }
+                TMR2_COUNT_REFERENCE++;
+                if (TMR2_COUNT_REFERENCE >= TMR2_COUNT_REFERENCE * REF_NUM) {
+                    REFERENCE_MODE = MODE_REF_DIRECT;
+                    TMR2_COUNT_REFERENCE = 0;
+                }
+                break;
+            }
+            
+            case MODE_REF_SQUARE_WAVE:
+            {
+                if (FLAG_REFERENCE_VALVE_PWM) {
+                    Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM;
+                    if (Ref_PWM >= REF_PWM) Ref_PWM = REF_MAG + REF_PWM;
+                    else Ref_PWM = -REF_MAG + REF_PWM;
+                }
+                if (FLAG_REFERENCE_VALVE_POSITION) {
+                    Ref_Valve_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_VALVE_POSITION;
+                    if (Ref_Valve_Pos >= REF_VALVE_POSITION) Ref_Valve_Pos = REF_MAG + REF_VALVE_POSITION;
+                    else Ref_Valve_Pos = -REF_MAG + REF_VALVE_POSITION;
+                }
+                if (FLAG_REFERENCE_JOINT_POSITION) {
+                    Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_POSITION;
+                    if (Ref_Joint_Pos >= REF_POSITION) Ref_Valve_Pos = REF_MAG + REF_POSITION;
+                    else Ref_Joint_Pos = -REF_MAG + REF_POSITION;
+                }
+                if (FLAG_REFERENCE_JOINT_TORQUE) {
+                    Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_TORQUE;
+                    if (Ref_Joint_Torq >= REF_TORQUE) Ref_Valve_Pos = REF_MAG + REF_TORQUE;
+                    else Ref_Joint_Torq = -REF_MAG + REF_TORQUE;
+                }
+                TMR2_COUNT_REFERENCE++;
+                if (TMR2_COUNT_REFERENCE >= TMR2_COUNT_REFERENCE * REF_NUM) {
+                    REFERENCE_MODE = MODE_REF_DIRECT;
+                    TMR2_COUNT_REFERENCE = 0;
+                }
+                break;
+            }
+            
+            default:
+                break;
+        }
+        
+        
+        
         /*******************************************************
         ***     Valve Control 
         ********************************************************/
@@ -253,52 +451,6 @@
     TIM3->SR = 0x0;  // reset the status register
 }
 
-/*******************************************************************************
- *  REFERENCE MODE
- ******************************************************************************/
-enum _REFERENCE_MODE{
-    MODE_REF_NO_ACT = 0,                                //0 
-    MODE_REF_DIRECT,                                //1
-    MODE_REF_COS_INC,                                  //2
-    MODE_REF_LINE_INC,                                 //3
-    MODE_REF_SIN_WAVE,                                  //4
-    MODE_REF_SQUARE_WAVE,                                  //5
-};
-
-/*******************************************************************************
- *  CONTROL MODE
- ******************************************************************************/
-enum _CONTROL_MODE{
-    //control mode
-    MODE_NO_ACT = 0,                                    //0
-    MODE_VALVE_OPEN_LOOP,                               //1
-    MODE_VALVE_POSITION_CONTROL,                        //2
-    
-    MODE_JOINT_POSITION_TORQUE_CONTROL_PWM,             //3
-    MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION,  //4
-    MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING,        //5
-    
-    MODE_JOINT_POSITION_PRES_CONTROL_PWM,               //6
-    MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION,    //7
-    MODE_VALVE_POSITION_PRES_CONTROL_LEARNING,          //8
-        
-    MODE_TEST_CURRENT_CONTROL,                          //9
-    MODE_TEST_PWM_CONTROL,                              //10
-    
-    //utility
-    MODE_TORQUE_SENSOR_NULLING = 20,                    //20
-    MODE_VALVE_NULLING_AND_DEADZONE_SETTING,            //21
-    MODE_FIND_HOME,                                     //22
-    MODE_VALVE_GAIN_SETTING,                        //23
-    MODE_PRESSURE_SENSOR_NULLING,                       //24
-    MODE_PRESSURE_SENSOR_CALIB,                         //25
-    MODE_ROTARY_FRICTION_TUNING,                        //26
-    
-    MODE_DDV_POS_VS_PWM_ID = 30,                           //30
-    MODE_DDV_DEADZONE_AND_CENTER,                       //31
-    MODE_DDV_POS_VS_FLOWRATE,                           //32
-};
-
 void ValveControl(unsigned int ControlMode)
 {
     switch (ControlMode) {