![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LIGHT2
Diff: main.cpp
- 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