eeprom_test

Dependencies:   mbed FastPWM

Revision:
45:35fa6884d0c6
Parent:
44:fe7d5cfd2eea
Child:
46:2694daea349b
--- a/main.cpp	Tue Jan 14 09:11:20 2020 +0000
+++ b/main.cpp	Thu Jan 16 12:05:15 2020 +0000
@@ -76,11 +76,7 @@
 
 extern int CID_RX_CMD;
 extern int CID_RX_REF_POSITION;
-extern int CID_RX_REF_TORQUE;
-extern int CID_RX_REF_PRES_DIFF;
-extern int CID_RX_REF_VOUT;
-extern int CID_RX_REF_VALVE_POSITION;
-extern int CID_RX_REF_CURRENT;
+extern int CID_RX_REF_PWM;
 
 extern int CID_TX_INFO;
 extern int CID_TX_POSITION;
@@ -114,7 +110,7 @@
     MODE_VALVE_OPEN_LOOP,                               //1
     MODE_VALVE_POSITION_CONTROL,                        //2
 
-    MODE_JOINT_POSITION_TORQUE_CONTROL_PWM,             //3
+    MODE_JOINT_CONTROL,             //3
     MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION,  //4
     MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING,        //5
 
@@ -269,9 +265,7 @@
         Ref_Valve_Pos_FF = (float) VALVE_MIN_POS;
     }
 
-    //VELOCITY_COMP_GAIN = 20;
-    //CAN_TX_PRES((int16_t)(Ref_Valve_Pos_FF), (int16_t) (7));
-    Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER) + DDV_CENTER;
+    Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER);
 
     return Ref_Valve_Pos_FF;
 }
@@ -450,10 +444,34 @@
 float FREQ_TMR3 = (float)FREQ_1k;
 //float DT_TMR3 = (float)DT_5k;
 float DT_TMR3 = (float)DT_1k;
+int cnt_trans = 0;
 extern "C" void TIM3_IRQHandler(void)
 {
     if (TIM3->SR & TIM_SR_UIF ) {
         ENC_UPDATE();
+        
+        if(MODE_POS_FT_TRANS == 1){
+            alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans/(float)(3*TMR_FREQ_5k)))/2.0f;
+            cnt_trans++;
+            if(cnt_trans>3*TMR_FREQ_5k)
+                MODE_POS_FT_TRANS = 2;
+        }
+        if(MODE_POS_FT_TRANS == 3){
+            alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans/(float)(3*TMR_FREQ_5k)))/2.0f;
+            cnt_trans++;
+            if(cnt_trans>3*TMR_FREQ_5k)
+                MODE_POS_FT_TRANS = 0;
+        }
+        else if(MODE_POS_FT_TRANS == 2){
+            alpha_trans = 1.0;
+            cnt_trans = 0;
+        }
+        else{
+            alpha_trans = 0.0;
+            cnt_trans = 0;
+        }
+
+            
 
         // CONTROL LOOP ------------------------------------------------------------
 
@@ -473,42 +491,31 @@
                 break;
             }
 
-            case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: {
-                float PWM_RAW_POS_FB = 0.0f; // PWM by Position Feedback
-                float PWM_RAW_POS_FF = 0.0f; // PWM by Position Feedforward
-                float PWM_RAW_FORCE_FB = 0.0f; // PWM by Force Feedback
-
-                // feedback input for position control
-                pos.err = pos.ref - (float) pos.sen;
-                pos.err_diff = pos.err - pos.err_old;
-                pos.err_old = pos.err;
-                pos.err_sum += pos.err;
-                if (pos.err_sum > 1000) pos.err_sum = 1000;
-                if (pos.err_sum<-1000) pos.err_sum = -1000;
-                //            PWM_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff;
-                PWM_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err;
-                PWM_RAW_POS_FB = PWM_RAW_POS_FB * 0.01f;
+            case MODE_JOINT_CONTROL: {
+                
+                float VALVE_POS_RAW_FORCE_FB = 0.0f;
+                
+                pos.err = pos.ref - pos.sen; //[pulse]
+                vel.err = vel.ref - vel.sen; //[pulse/s]
+                double K_spring = 1.0f; //[N/mm]
+                double D_damper = 0.0001f;
+                torq.ref = torq.ref + (K_spring * pos.err + D_damper * vel.err) / ENC_PULSE_PER_POSITION; //[N]
+                
+                // torque feedback
+                torq.err = torq.ref - torq.sen; //[pulse]
+                torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse]
+                if (torq.err_sum > 1000) torq.err_sum = 1000;
+                if (torq.err_sum<-1000) torq.err_sum = -1000;
+                
 
-                // feedforward input for position control
-                float Ref_Vel_Act = vel.ref/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
-                float K_ff = 0.9f;
-                if(Ref_Vel_Act > 0) K_ff = 0.90f; // open
-                if(Ref_Vel_Act > 0) K_ff = 0.75f; // close
-                PWM_RAW_POS_FF = K_ff*Ref_Vel_Act/0.50f;
+                VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) / (float) TORQUE_SENSOR_PULSE_PER_TORQUE * 0.01f + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err /(float) ENC_PULSE_PER_POSITION * 0.01f + DDV_JOINT_POS_FF(vel.ref));   
 
-                // torque feedback
-                //            torq.err = torq.ref - torq.sen;
-                //            torq.err_diff = torq.err - torq.err_old;
-                //            torq.err_old = torq.err;
-                //            torq.err_sum += torq.err;
-                //            if (torq.err_sum > 1000) torq.err_sum = 1000;
-                //            if (torq.err_sum<-1000) torq.err_sum = -1000;
-                //            VALVE_PWM_RAW_TORQ = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
-                //            VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f;
-
-                PWM_RAW_FORCE_FB = 0.0f;
-
-                V_out = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB;
+                if (VALVE_POS_RAW_FORCE_FB >= 0) {
+                    valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
+                } else{
+                    valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS;
+                }
+                VALVE_POS_CONTROL(valve_pos.ref);
 
                 break;
             }
@@ -549,10 +556,10 @@
 
                 valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
 
-                if (valve_pos.ref > DDV_CENTER) {
-                    valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS - DDV_CENTER;
-                } else if(valve_pos.ref < DDV_CENTER) {
-                    valve_pos.ref = valve_pos.ref - DDV_CENTER + VALVE_DEADZONE_MINUS;
+                if (valve_pos.ref >= 0) {
+                    valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS;
+                } else if(valve_pos.ref < 0) {
+                    valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_MINUS;
                 }
                 VALVE_POS_CONTROL(valve_pos.ref);
 
@@ -651,30 +658,6 @@
             }
 
             case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING: {
-                
-                float VALVE_POS_RAW_FORCE_FB = 0.0f;
-                
-                pos.err = pos.ref - pos.sen; //[pulse]
-                vel.err = vel.ref - vel.sen; //[pulse/s]
-                double K_spring = 1.0f;
-                double D_damper = 0.0001f;
-                torq.ref = (K_spring * pos.err + D_damper * vel.err) * ENC_PULSE_PER_POSITION; //[Nm]
-                
-                // torque feedback
-                torq.err = torq.ref - torq.sen; //[pulse]
-                torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse]
-                if (torq.err_sum > 100) torq.err_sum = 100;
-                if (torq.err_sum<-100) torq.err_sum = -100;
-                VALVE_POS_RAW_FORCE_FB = ((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) * (float) TORQUE_SENSOR_PULSE_PER_TORQUE;
-
-                valve_pos.ref = DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
-
-                if (valve_pos.ref > DDV_CENTER) {
-                    valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS - 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;
             }