Revision 7:e9086c72bb22, committed 2019-08-20

Comitter:

jobuuu

Date:

Tue Aug 20 12:27:19 2019 +0000

Parent:

6:df07d3491e3a

Child:

8:5d2eebdad025

Commit message:

Updated : 20190820;

Changed in this revision

--- a/CAN/function_CAN.cpp	Tue Aug 20 10:40:27 2019 +0000
+++ b/CAN/function_CAN.cpp	Tue Aug 20 12:27:19 2019 +0000
@@ -1,4 +1,3 @@
-#include "mbed.h"
 #include "function_CAN.h"
 
 // CAN ID Setting Variables
@@ -6,18 +5,26 @@
 int CID_RX_REF_POSITION = 200;
 int CID_RX_REF_TORQUE = 300;
 int CID_RX_REF_PRES_DIFF = 400;
-int CID_RX_REF_PWM = 500;
+int CID_RX_REF_VOUT = 500;
 int CID_RX_REF_VALVE_POSITION = 600;
 
 int CID_TX_INFO = 1100;
 int CID_TX_POSITION = 1200;
 int CID_TX_TORQUE = 1300;
 int CID_TX_PRES = 1400;
-int CID_TX_PWM = 1500;
+int CID_TX_VOUT = 1500;
 int CID_TX_VALVE_POSITION = 1600;
 
-extern CAN can;
-extern CANMessage msg;
+// Board Information 
+int BNO = 0;
+int CONTROL_MODE = 0;
+double P_GAIN_JOINT_POSITION = 0.0;
+double I_GAIN_JOINT_POSITION = 0.0;
+double D_GAIN_JOINT_POSITION = 0.0;
+double P_GAIN_JOINT_TORQUE = 0.0;
+double I_GAIN_JOINT_TORQUE = 0.0;
+double D_GAIN_JOINT_TORQUE = 0.0;
+
 
 void ReadCMD(char CMD)
 {
@@ -38,10 +45,28 @@
     can.read(msg);
     unsigned int address = msg.id;
     
-    if(address==100){
+    if(address==CID_RX_CMD){
         unsigned int CMD = msg.data[0];
         ReadCMD(CMD);
+    } else if(address==CID_RX_REF_POSITION) {
+        int32_t temp_pos = (int32_t) (msg.data[0] | msg.data[1] << 8 | msg.data[2] << 16 | msg.data[3] << 24);
+        int32_t temp_vel = (int32_t) (msg.data[4] | msg.data[5] << 8 | msg.data[6] << 16 | msg.data[7] << 24);
+        pos.ref = (double)temp_pos;
+        vel.ref = (double)temp_vel;
+    } else if(address==CID_RX_REF_TORQUE) {
+        int16_t temp_torq = (int16_t) (msg.data[0] | msg.data[1] << 8);
+        torq.ref = (double)temp_torq;
+    } else if(address==CID_RX_REF_PRES_DIFF) {
+//        int16_t temp_presdiff = (int16_t) (msg.data[0] | msg.data[1] << 8);
+//        torq.ref = (double)temp_presdiff;   
+    } else if(address==CID_RX_REF_VOUT) {
+        int16_t temp_PWM = (int16_t) (msg.data[0] | msg.data[1] << 8);
+        Vout.ref = (double)temp_PWM;   
+    } else if(address==CID_RX_REF_VALVE_POSITION) {
+        int16_t temp_cur = (int16_t) (msg.data[0] | msg.data[1] << 8);
+        cur.ref = (double)temp_cur; 
     }
+        
 }
 
 /******************************************************************************
@@ -98,7 +123,7 @@
 
     can.write(temp_msg);
 }
-
+*/
 inline void CAN_TX_CONTROL_MODE(void) {
     CANMessage temp_msg;
 
@@ -109,7 +134,7 @@
 
     can.write(temp_msg);
 }
-
+/*
 inline void CAN_TX_JOINT_ENC_DIR(void) {
     CANMessage temp_msg;
 
@@ -173,17 +198,18 @@
 
     can.write(temp_msg);
 }
+*/
 
 inline void CAN_TX_PID_GAIN(int t_type) {
     // t_type = 0 : valve position control gain
     // t_type = 1 : joint position control gain
     // t_type = 2 : joint torque control gain
 
-    long sendPgain, sendIgain, sendDgain;
+    long sendPgain=0, sendIgain=0, sendDgain=0;
     if (t_type == 0) {
-        sendPgain = (long) (P_GAIN_VALVE_POSITION);
-        sendIgain = (long) (I_GAIN_VALVE_POSITION);
-        sendDgain = (long) (D_GAIN_VALVE_POSITION);
+//        sendPgain = (long) (P_GAIN_VALVE_POSITION);
+//        sendIgain = (long) (I_GAIN_VALVE_POSITION);
+//        sendDgain = (long) (D_GAIN_VALVE_POSITION);
     } else if (t_type == 1) {
         sendPgain = (long) (P_GAIN_JOINT_POSITION);
         sendIgain = (long) (I_GAIN_JOINT_POSITION);
@@ -210,6 +236,7 @@
     can.write(temp_msg);
 }
 
+/*
 inline void CAN_TX_VALVE_DEADZONE(void) {
     CANMessage temp_msg;
 

--- a/CAN/function_CAN.h	Tue Aug 20 10:40:27 2019 +0000
+++ b/CAN/function_CAN.h	Tue Aug 20 12:27:19 2019 +0000
@@ -3,6 +3,9 @@
 
 #include "mbed.h"
 
+extern CAN can;
+extern CANMessage msg;
+
 // CID_RX_CMD - RX CMD type
 #define             CRX_ASK_INFO                    0
 #define             CRX_ASK_BNO                     1
@@ -72,6 +75,41 @@
 #define             CRX_SET_ERR_CLEAR               150
 #define             CRX_JUMP_STATUS                 255
 
+// CID_TX_INFO - TX INFO type
+
+#define             CTX_SEND_INFO                               0
+#define             CTX_SEND_BNO                                1
+#define             CTX_SEND_OPERATING_MODE                     2
+#define             CTX_SEND_CAN_FREQ                           6
+#define             CTX_SEND_CONTROL_MODE                       7
+#define             CTX_SEND_JOINT_ENC_DIR                      9
+#define             CTX_SEND_VALVE_DIR                          10
+#define             CTX_SEND_VALVE_ENC_DIR                      11
+#define             CTX_SEND_VOLTAGE_SUPPLY                     12
+#define             CTX_SEND_VOLTAGE_VALVE                      13
+#define             CTX_SEND_PID_GAIN                           20
+#define             CTX_SEND_VALVE_DEADZONE                     21
+#define             CTX_SEND_VELOCITY_COMP_GAIN                 22
+#define             CTX_SEND_COMPLIANCE_GAIN                    23
+#define             CTX_SEND_VALVE_CNETER                       24
+#define             CTX_SEND_VALVE_FF                           25
+#define             CTX_SEND_BULK_MODULUS                       26
+#define             CTX_SEND_CHAMBER_VOLUME                     27
+#define             CTX_SEND_PISTON_AREA                        28
+#define             CTX_SEND_PRES                               29
+#define             CTX_SEND_ENC_LIMIT                          30
+#define             CTX_SEND_STROKE                             31
+#define             CTX_SEND_VALVE_LIMIT                        32
+#define             CTX_SEND_ENC_PULSE_PER_POSITION             33
+#define             CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE     34
+#define             CTX_SEND_PRES_SENSOR_PULSE_PER_BAR          35
+#define             CTX_SEND_FRICTION                           36
+#define             CTX_SEND_VALVE_GAIN_PLUS                    37
+#define             CTX_SEND_VALVE_GAIN_MINUS                   38
+#define             CTX_SEND_REFENCE_MODE                       39
+#define             CTX_SEND_HOMEPOS_OFFSET                     40
+#define             CTX_SEND_HOMEPOS_VALVE_OPENING              41
+
 // Information Transmission
 //inline void CAN_TX_INFO(void);
 //inline void CAN_TX_BNO(void);
@@ -97,6 +135,48 @@
 inline void CAN_TX_PWM(int16_t t_pwm);
 inline void CAN_TX_VALVE_POSITION(int16_t t_valve_pos);
 
+// Board Information Variables
+extern int BNO;
+extern int CONTROL_MODE;
+extern double P_GAIN_JOINT_POSITION;
+extern double I_GAIN_JOINT_POSITION;
+extern double D_GAIN_JOINT_POSITION;
+extern double P_GAIN_JOINT_TORQUE;
+extern double I_GAIN_JOINT_TORQUE;
+extern double D_GAIN_JOINT_TORQUE;
+
+class State 
+{
+    public:
+        double sen;
+        double ref;
+        double ref_old;
+        double ref_diff;
+        double err;
+        double err_int;
+        double err_old;
+        double err_diff;
+    public:
+        State(){
+            sen = 0.0;
+            ref = 0.0;
+            ref_old = 0.0;
+            ref_diff = 0.0;
+            err = 0.0;
+            err_int = 0.0;
+            err_old = 0.0;
+            err_diff = 0.0;
+        }
+};
+
+extern State pos;
+extern State vel;
+extern State Vout;
+extern State torq;
+extern State pres_A;
+extern State pres_B;
+extern State cur;
+
 // CAN Receive Functions
 void ReadCMD();
 void CAN_RX_HANDLER();

--- 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

--- a/setting.h	Tue Aug 20 10:40:27 2019 +0000
+++ b/setting.h	Tue Aug 20 12:27:19 2019 +0000
@@ -3,11 +3,6 @@
 
 #define PI          3.141592
 
-extern DigitalOut check;
-extern DigitalOut check_2;
-extern AnalogOut dac_1;
-extern AnalogOut dac_2;
-
 // pwm 
 #define PIN_V       PB_7
 #define PIN_W       PB_6
@@ -25,6 +20,11 @@
 #define DT_20k      0.00005
 #define DT_40k      0.000025
 
+extern DigitalOut check;
+extern DigitalOut check_2;
+extern AnalogOut dac_1;
+extern AnalogOut dac_2;
+
 extern double dtc_v;
 extern double dtc_w;
 
@@ -40,4 +40,16 @@
 extern DigitalOut enc_cs;
 
 // UART 
-extern Serial pc; //Serial pc(PA_9,PA_10); _ UART
\ No newline at end of file
+extern Serial pc; //Serial pc(PA_9,PA_10); _ UART
+
+// CAN
+extern CAN can;
+extern CANMessage msg;
+
+// Board Information 
+
+
+// User Function 
+
+void CurrentControl();
+void ValveControl(unsigned int ControlMode);