2011

Dependencies:   mbed FastPWM

Revision:
34:bb2ca2fc2a8e
Parent:
33:91b17819ec30
Child:
35:34ce7b0347b8
--- a/main.cpp	Tue Nov 12 11:29:36 2019 +0000
+++ b/main.cpp	Tue Nov 19 13:13:28 2019 +0000
@@ -192,15 +192,15 @@
     can.attach(&CAN_RX_HANDLER);
     CAN_ID_INIT();
     make_delay();
-    
+
     //Timer priority
     NVIC_SetPriority(TIM3_IRQn, 2);
     NVIC_SetPriority(TIM2_IRQn, 3);
     NVIC_SetPriority(TIM4_IRQn, 4);
-    
+
     //can.reset();
     can.filter(msg.id, 0xFFFFF000, CANStandard);
-    
+
     // spi _ enc
     spi_enc_set_init();
     make_delay();
@@ -258,9 +258,9 @@
     } else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
         Ref_Valve_Pos_FF = (float) VALVE_MIN_POS;
     }
-    
+
     Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER) + DDV_CENTER;
-    
+
     return Ref_Valve_Pos_FF;
 }
 
@@ -283,8 +283,8 @@
     } else if(REF_VALVE_POS < VALVE_MIN_POS) {
         REF_VALVE_POS = VALVE_MIN_POS;
     }
-    
-    
+
+
 //    if(REF_VALVE_POS >= VALVE_POS_VS_PWM[0])
 //    {
 //        if(REF_VALVE_POS <=  VALVE_POS_VS_PWM[1]) {
@@ -329,19 +329,19 @@
 
 #define LT_MAX_IDX  57
 float LT_PWM_duty[LT_MAX_IDX] = {-100.0f, -80.0f, -60.0f, -50.0f, -40.0f, -35.0f, -30.0f, -25.0f, -20.0f,
-                                  -19.0f, -18.0f, -17.0f, -16.0f, -15.0f, -14.0f, -13.0f, -12.0f, -11.0f, -10.0f,
-                                  -9.0f, -8.0f, -7.0f, -6.0f, -5.0f, -4.0f, -3.0f, -2.0f, -1.0f, 0.0f,
-                                  1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, 10.0f,
-                                  11.0f, 12.0f, 13.0f, 14.0f, 15.0f, 16.0f, 17.0f, 18.0f, 19.0f, 20.0f,
-                                  25.0f, 30.0f, 35.0f, 40.0f, 50.0f, 60.0f, 80.0f, 100.0f
-                                 };  // duty
+                                 -19.0f, -18.0f, -17.0f, -16.0f, -15.0f, -14.0f, -13.0f, -12.0f, -11.0f, -10.0f,
+                                 -9.0f, -8.0f, -7.0f, -6.0f, -5.0f, -4.0f, -3.0f, -2.0f, -1.0f, 0.0f,
+                                 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, 10.0f,
+                                 11.0f, 12.0f, 13.0f, 14.0f, 15.0f, 16.0f, 17.0f, 18.0f, 19.0f, 20.0f,
+                                 25.0f, 30.0f, 35.0f, 40.0f, 50.0f, 60.0f, 80.0f, 100.0f
+                                };  // duty
 float LT_Voltage_Output[LT_MAX_IDX] = {-321.4f, -291.3f, -261.5f, -246.8f, -231.7f, -223.9f, -216.1f, -207.9f, -198.8f,
-                                        -196.9f, -195.0f, -192.5f, -188.8f, -184.5f, -180.2f, -175.9f, -171.5f, -166.3f, -161.0f,
-                                        -156.0f, -149.5f, -139.0f, -126.0f, -107.0f, -87.5f, -64.0f, -38.5f, -9.4f, 0.0f,
-                                        12.0f, 43.5f, 69.0f, 94.0f, 114.0f, 132.0f, 146.0f, 155.5f, 162.3f, 168.2f,
-                                        173.1f, 178.2f, 182.8f, 187.4f, 191.8f, 196.0f, 199.7f, 201.9f, 203.8f, 205.6f,
-                                        214.6f, 222.5f, 230.4f, 238.2f, 253.3f, 268.0f, 297.6f, 327.7f
-                                       }; // mV
+                                       -196.9f, -195.0f, -192.5f, -188.8f, -184.5f, -180.2f, -175.9f, -171.5f, -166.3f, -161.0f,
+                                       -156.0f, -149.5f, -139.0f, -126.0f, -107.0f, -87.5f, -64.0f, -38.5f, -9.4f, 0.0f,
+                                       12.0f, 43.5f, 69.0f, 94.0f, 114.0f, 132.0f, 146.0f, 155.5f, 162.3f, 168.2f,
+                                       173.1f, 178.2f, 182.8f, 187.4f, 191.8f, 196.0f, 199.7f, 201.9f, 203.8f, 205.6f,
+                                       214.6f, 222.5f, 230.4f, 238.2f, 253.3f, 268.0f, 297.6f, 327.7f
+                                      }; // mV
 
 float PWM_duty_byLT(float Ref_V)
 {
@@ -499,8 +499,8 @@
             }
 
             case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: {
-                
-            
+
+
                 float VALVE_POS_RAW_POS_FB = 0.0f; // Valve Position by Position Feedback
                 //float VALVE_POS_RAW_POS_FF = 0.0f; // Valve Position by Position Feedforward
                 float VALVE_POS_RAW_FORCE_FB = 0.0f; // Valve Position by Force Feedback
@@ -537,18 +537,15 @@
 
                 valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
                 //valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
-                
-                if (valve_pos.ref > DDV_CENTER)
-                {
+
+                if (valve_pos.ref > DDV_CENTER) {
                     valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS - DDV_CENTER;
-                }
-                else if(valve_pos.ref < DDV_CENTER)
-                {
+                } else if(valve_pos.ref < DDV_CENTER) {
                     valve_pos.ref = valve_pos.ref - DDV_CENTER + VALVE_DEADZONE_MINUS;
                 }
                 VALVE_POS_CONTROL(valve_pos.ref);
-                
-                
+
+
                 break;
             }
 
@@ -966,7 +963,7 @@
                         VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01f;
                         valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
                         VALVE_POS_CONTROL(valve_pos.ref);
-                        
+
                         //float wn_Pos = 2.0f*PI*5.0f; // f_cut : 10Hz Position Control
                         //I_REF = 0.04f*wn_Pos*((float)joint_pos_err/ENC_PULSE_PER_POSITION);
                         ////       L velocity >> mA convert
@@ -1408,7 +1405,7 @@
                                 VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
 
                                 ROM_RESET_DATA();
-                
+
                                 CONTROL_MODE = MODE_NO_ACT;
                                 DZ_index = 1;
                             }
@@ -1501,7 +1498,7 @@
                                 first_check = 0;
                                 VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
                                 VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
-                                
+
                                 CONTROL_MODE = MODE_NO_ACT;
                                 DZ_index = 1;
                             }
@@ -1625,10 +1622,9 @@
         /*******************************************************
         ***     PWM
         ********************************************************/
-        if (V_out >= 12000.0f){
+        if (V_out >= 12000.0f) {
             V_out = 12000.0f;
-        }
-        else if(V_out<=-12000.0f){
+        } else if(V_out<=-12000.0f) {
             V_out = -12000.0f;
         }
         PWM_out= V_out/SUPPLY_VOLTAGE; // Full duty : 12000.0mV
@@ -1758,3 +1754,4 @@
 
 
 
+