2011

Dependencies:   mbed FastPWM

Revision:
7:e9086c72bb22
Parent:
6:df07d3491e3a
Child:
8:5d2eebdad025
--- a/main.cpp	Tue Aug 20 10:40:27 2019 +0000
+++ b/main.cpp	Tue Aug 20 12:27:19 2019 +0000
@@ -6,54 +6,44 @@
 #include "I2C_AS5510.h"
 #include "setting.h"
 
-// dac & check
+// dac & check ///////////////////////////////////////////
 DigitalOut check(PC_2);
 DigitalOut check_2(PC_3);
 AnalogOut dac_1(PA_4);
 AnalogOut dac_2(PA_5);
 //AnalogIn adc3(PC_1);
 
-// pwm
+// PWM ///////////////////////////////////////////
 double dtc_v=0.0;
 double dtc_w=0.0;
 
-// I2C
+// I2C ///////////////////////////////////////////
 I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F)
 const int i2c_slave_addr1 =  0x56;
 unsigned int value; // 10bit output of reading sensor AS5510
 
-// SPI
+// SPI ///////////////////////////////////////////
 SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
 DigitalOut eeprom_cs(PB_12);
 SPI enc(PC_12,PC_11,PC_10);
 DigitalOut enc_cs(PD_2);
 DigitalOut indi_led(PA_15);
 
-// UART
+// UART ///////////////////////////////////////////
 Serial pc(PA_9,PA_10); //  _ UART
 
-//CAN
+// CAN ///////////////////////////////////////////
 CAN can(PB_8, PB_9, 1000000);
 CANMessage msg;
 
-// Variables
-double cur = 0.0;
-double cur_ref = 0.0;
-double cur_ref_old = 0.0;
-double cur_ref_diff = 0.0;
-double cur_err = 0.0;
-double cur_err_int = 0.0;
-double cur_err_old = 0.0;
-double cur_err_diff = 0.0;
-
-double pos = 0.0;
-double pos_ref = 0.0;
-
-double vel;
-double vel_ref;
-
-double pres_A;
-double pres_B;
+// Variables ///////////////////////////////////////////
+State pos;
+State vel;
+State Vout;
+State torq;
+State pres_A;
+State pres_B;
+State cur;
 
 double V_out=0.0;
 double V_rem=0.0; // for anti-windup
@@ -61,6 +51,10 @@
 
 double PWM_out=0.0;
 
+// =============================================================================
+// =============================================================================
+// =============================================================================
+
 int main()
 {
     /*********************************
@@ -168,7 +162,8 @@
                     
             double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
             double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
-            cur=cur*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
+            cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
+
         }
 
         //DAC
@@ -178,49 +173,17 @@
         /*******************************************************
         ***     Valve Control 
         ********************************************************/
+        ValveControl(CONTROL_MODE);
         
-        bool FLAG_current_control = false;
-        if(FLAG_current_control) {
-            cur_err = cur_ref - cur;
-            cur_err_int = cur_err_int + cur_err*DT_TMR4;
-            cur_err_diff = (cur_err - cur_err_old)*FREQ_TMR4;
-            cur_err_old = cur_err;
-            
-            double R_model = 150.0; // ohm
-            double L_model = 0.3;
-            double w0 = 2.0*3.14*90.0;
-            double KP_I = L_model*w0;
-            double KI_I = R_model*w0;
-            double KD_I = 0.0;
-            
-            double FF_gain = 0.0;
-            V_out = (int) (KP_I * cur_err + KI_I * cur_err_int + KD_I * cur_err_diff);
-//          V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV
-            V_out = V_out + FF_gain * (R_model*cur_ref + L_model*cur_ref_diff); // Unit : mV
-            
-            double Ka = 5.0/KP_I; 
-            if(V_out > V_MAX) {
-                V_rem = V_out-V_MAX;
-                V_rem = Ka*V_rem;
-                V_out = V_MAX;
-                cur_err_int = cur_err_int - V_rem*DT_5k;
-            } else if(V_out < -V_MAX) {
-                V_rem = V_out-(-V_MAX);
-                V_rem = Ka*V_rem; 
-                V_out = -V_MAX;
-                cur_err_int = cur_err_int - V_rem*DT_5k;
-            }        
-        
-        } else {
-            // PWM_RAW : -5000.0mV~5000.0mV(full duty)
-            double t = (double)CNT_TMR4*DT_TMR4;
-            double T = 5.0; 
-            
-            V_out = 1000.0*sin(2.0*PI*t/T); // Unit : mV
-//          if(V_out > 0.0) V_out = 1000.0;
-//          else if(V_out < 0.0) V_out = -1000.0; 
-        }
+        double t = (double)CNT_TMR4*DT_TMR4;
+        double T = 5.0; 
+        V_out = 1000.0*sin(2.0*PI*t/T); // V_out : -5000.0mV~5000.0mV(full duty)
+//      if(V_out > 0.0) V_out = 1000.0;
+//      else if(V_out < 0.0) V_out = -1000.0; 
 
+        /*******************************************************
+        ***     PWM
+        ********************************************************/
         PWM_out= V_out/12000.0; // Full duty : 12000.0mV
         
         // Saturation of output voltage to 5.0V
@@ -245,7 +208,7 @@
         if((CNT_TMR4%40)==0){
             msg.id = 50;
             msg.len = 4;
-            int temp_CUR = (int)(cur*1000.0);
+            int temp_CUR = (int)(cur.sen*1000.0);
             msg.data[0]=0x00FF&temp_CUR;
             msg.data[1]=0x00FF&(temp_CUR>>8);
             int temp_PWM = (int)(V_out);
@@ -264,4 +227,120 @@
         CNT_TMR4++; 
     }
     TIM4->SR = 0x0;  // reset the status register
+}
+
+
+/*******************************************************************************
+ *  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
+};
+
+void ValveControl(unsigned int ControlMode)
+{
+    switch (ControlMode) {
+        case MODE_NO_ACT: // 0
+            V_out = 0.0;
+            break;
+        case MODE_VALVE_OPEN_LOOP: // 1
+            V_out = Vout.ref;
+            break;      
+        case MODE_VALVE_POSITION_CONTROL: // 2
+            CurrentControl();
+            break;  
+        case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: // 3
+            V_out = 0.0;
+            break;      
+        case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: // 4
+            double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
+            double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward
+            
+            // feedback input for position control
+            pos.err = pos.ref - pos.sen;
+            double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.0*3.1415*50.0)); // f_cutoff : 50Hz
+            double err_diff = (pos.err - pos.err_old)*(double)FREQ_5k;
+            pos.err_diff = (1.0-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
+            pos.err_old = pos.err;
+            I_REF_POS_FB = 0.001*((double)P_GAIN_JOINT_POSITION * pos.err + (double)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1);
+            
+            // feedforward input for position control
+            double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s] 
+            double K_ff = 1.3;
+            double K_v = 0.0;
+            if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA)
+            if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA) 
+            I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
+                        
+            cur.ref = I_REF_POS_FF + I_REF_POS_FB;
+            break;  
+        case MODE_TEST_CURRENT_CONTROL: // 9
+            V_out = 0.0;
+            break;  
+        case MODE_TEST_PWM_CONTROL: // 10
+            V_out = 0.0;
+            break;   
+        case MODE_FIND_HOME: // 22
+            V_out = 0.0;
+            break;                                             
+        default:
+            V_out = 0.0;
+            break;
+    
+    }
+}
+
+void CurrentControl() {
+    cur.err = cur.ref - cur.sen;
+    cur.err_int = cur.err_int + cur.err*DT_TMR4;
+    cur.err_diff = (cur.err - cur.err_old)*FREQ_TMR4;
+    cur.err_old = cur.err;
+
+    double R_model = 150.0; // ohm
+    double L_model = 0.3;
+    double w0 = 2.0*3.14*90.0;
+    double KP_I = L_model*w0;
+    double KI_I = R_model*w0;
+    double KD_I = 0.0;
+
+    double FF_gain = 0.0;
+    V_out = (int) (KP_I * cur.err + KI_I * cur.err_int + KD_I * cur.err_diff);
+    //          V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV
+    V_out = V_out + FF_gain * (R_model*cur.ref + L_model*cur.ref_diff); // Unit : mV
+            
+    double Ka = 5.0/KP_I; 
+    if(V_out > V_MAX) {
+        V_rem = V_out-V_MAX;
+        V_rem = Ka*V_rem;
+        V_out = V_MAX;
+        cur.err_int = cur.err_int - V_rem*DT_5k;
+    } else if(V_out < -V_MAX) {
+        V_rem = V_out-(-V_MAX);
+        V_rem = Ka*V_rem; 
+        V_out = -V_MAX;
+        cur.err_int = cur.err_int - V_rem*DT_5k;
+    }   
 }
\ No newline at end of file