Sungwoo Kim
2011
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