Sungwoo Kim
2011
Diff: main.cpp
- Revision:
- 223:c831718303c9
- Parent:
- 222:bd00f763acb1
- Child:
- 224:fd7961dfa78a
diff -r bd00f763acb1 -r c831718303c9 main.cpp
--- a/main.cpp Mon Jun 07 01:17:25 2021 +0000
+++ b/main.cpp Mon Jun 14 10:36:29 2021 +0000
@@ -2,7 +2,7 @@
//distributed by Sungwoo Kim
// 2020/12/28
//revised by Buyoun Cho
-// 2021/04/20
+// 2021/04/20
// 유의사항
// 소수 적을때 뒤에 f 꼭 붙이기
@@ -95,10 +95,12 @@
float temp_P_GAIN = 0.0f;
float temp_I_GAIN = 0.0f;
int temp_VELOCITY_COMP_GAIN = 0;
+int logging = 0;
-inline float tanh_inv(float y) {
- if(y >= 1.0f - 0.000001f) y = 1.0f - 0.000001f;
- if(y <= -1.0f + 0.000001f) y = -1.0f + 0.000001f;
+inline float tanh_inv(float y)
+{
+ if(y >= 1.0f - 0.000001f) y = 1.0f - 0.000001f;
+ if(y <= -1.0f + 0.000001f) y = -1.0f + 0.000001f;
return log(sqrt((1.0f+y)/(1.0f-y)));
}
@@ -107,9 +109,9 @@
* REFERENCE MODE
******************************************************************************/
enum _REFERENCE_MODE {
- MODE_REF_NO_ACT = 0,
- MODE_REF_DIRECT,
- MODE_REF_FINDHOME
+ MODE_REF_NO_ACT = 0,
+ MODE_REF_DIRECT,
+ MODE_REF_FINDHOME
};
/*******************************************************************************
@@ -441,7 +443,7 @@
// Current ===================================================
//ADC3->CR2 |= 0x40000000; // adc _ 12bit
-
+
cur.UpdateSen(((float)ADC3->DR-2047.5f)/2047.5f*10.0f, FREQ_TMR4, 500.0f); // unit : mA
// Encoder ===================================================
@@ -466,7 +468,7 @@
pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f);
if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
- float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm
+ float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm
torq.UpdateSen(torq_new,FREQ_TMR4,1000.0f); // unit : Nm
} else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N
@@ -541,7 +543,7 @@
if (((OPERATING_MODE&0b010)>>1) == 0) {
K_v = 1.03f; // Q = K_v*sqrt(deltaP)*tanh(C_d*Xv);
- K_v = 0.16f;
+ C_d = 0.16f;
mV_PER_mA = 500.0f; // 5000mV/10mA
mV_PER_pulse = 0.5f; // 5000mV/10000pulse
mA_PER_pulse = 0.001f; // 10mA/10000pulse
@@ -551,7 +553,7 @@
mV_PER_pulse = 0.5f; // 5000mV/10000pulse
mA_PER_pulse = 0.003f; // 30mA/10000pulse
}
-
+
// =====================================================================
// CONTROL LOOP --------------------------------------------------------
// =====================================================================
@@ -571,12 +573,11 @@
break;
}
- case MODE_TORQUE_SENSOR_NULLING:
- {
- static float FORCE_pulse_sum = 0.0;
- static float PresA_pulse_sum = 0.0;
- static float PresB_pulse_sum = 0.0;
-
+ case MODE_TORQUE_SENSOR_NULLING: {
+ static float FORCE_pulse_sum = 0.0;
+ static float PresA_pulse_sum = 0.0;
+ static float PresB_pulse_sum = 0.0;
+
// DAC Voltage reference set
float VREF_TuningGain = -0.000003f;
if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 5) {
@@ -592,7 +593,7 @@
if (FORCE_VREF < 0.0f) FORCE_VREF = 0.0f;
dac_1 = FORCE_VREF / 3.3f;
}
- } else if (SENSING_MODE == 1) { // Pressure Sensor
+ } else if (SENSING_MODE == 1) { // Pressure Sensor
PresA_pulse_sum += pres_A.sen*PRES_SENSOR_A_PULSE_PER_BAR;
PresB_pulse_sum += pres_B.sen*PRES_SENSOR_B_PULSE_PER_BAR;
if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
@@ -607,7 +608,7 @@
dac_1 = PRES_A_VREF / 3.3f;
PRES_B_VREF += VREF_TuningGain * (0.0f - PresB_pluse_mean);
if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f;
- if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
+ if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
dac_2 = PRES_B_VREF / 3.3f;
}
}
@@ -631,14 +632,13 @@
break;
}
- case MODE_FIND_HOME:
- {
+ case MODE_FIND_HOME: {
static int cnt_findhome = 0;
static int cnt_terminate_findhome = 0;
static float FINDHOME_POSITION_pulse = 0.0f;
static float FINDHOME_POSITION_pulse_OLD = 0.0f;
static float FINDHOME_VELOCITY_pulse = 0.0f;
- static float REF_POSITION_FINDHOME_INIT = 0.0f;
+ static float REF_POSITION_FINDHOME_INIT = 0.0f;
if (FINDHOME_STAGE == FINDHOME_INIT) {
REFERENCE_MODE = MODE_REF_FINDHOME;
@@ -666,7 +666,7 @@
if ((cnt_terminate_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec
double GOTOHOME_SPEED = 10.0f; // 20mm/s or 20deg/s
if (HOMEPOS_OFFSET > 0) {
- REF_POSITION_FINDHOME = REF_POSITION_FINDHOME + GOTOHOME_SPEED*DT_5k;
+ REF_POSITION_FINDHOME = REF_POSITION_FINDHOME + GOTOHOME_SPEED*DT_5k;
} else {
REF_POSITION_FINDHOME = REF_POSITION_FINDHOME - GOTOHOME_SPEED*DT_5k;
}
@@ -678,24 +678,24 @@
FINDHOME_POSITION_pulse = 0;
FINDHOME_POSITION_pulse_OLD = 0;
FINDHOME_VELOCITY_pulse = 0;
-
+
cnt_findhome = 0;
cnt_terminate_findhome = 0;
pos.ref = 0.0f;
FINDHOME_STAGE = FINDHOME_ZEROPOSE;
}
} else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) {
-
+
// int T_move = 2*TMR_FREQ_5k;
int T_move = 10000;
REF_POSITION_FINDHOME = ((0.0f - REF_POSITION_FINDHOME_INIT)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)REF_POSITION_FINDHOME_INIT)/ENC_PULSE_PER_POSITION;
-
+
cnt_findhome++;
-
+
REFERENCE_MODE = MODE_REF_FINDHOME;
CONTROL_MODE = MODE_JOINT_CONTROL;
alpha_trans = 0.0f;
-
+
if (cnt_findhome >= T_move) {
cnt_findhome = 0;
pos.ref = 0.0f;
@@ -722,7 +722,7 @@
VALVE_POS_CONTROL(valve_pos.ref);
V_out = Vout.ref;
} else if (CURRENT_CONTROL_MODE == 0) { //PWM
- V_out = valve_pos.ref;
+ V_out = valve_pos.ref;
} else {
I_REF = valve_pos.ref * 0.001f; // Unit : pulse >> mA
float I_MAX = 10.0f; // Max : 10mA
@@ -735,56 +735,56 @@
break;
}
- case MODE_JOINT_CONTROL:
- {
+ case MODE_JOINT_CONTROL: {
float temp_vel_pos = 0.0f; // desired velocity for position control
- float temp_vel_FT = 0.0f; // desired velocity for force/torque control
- float temp_vel_ff = 0.0f; // desired velocity for feedforward control
+ float temp_vel_FT = 0.0f; // desired velocity for force/torque control
+ float temp_vel_ff = 0.0f; // desired velocity for feedforward control
float temp_vel = 0.0f;
float wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
-
+
pos.err = pos.ref - pos.sen; // Unit : mm or deg
vel.err = vel.ref - vel.sen; // Unit : mm/s or deg/s
-
+
// position control command ===============================================================================================================================================
- if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
- temp_vel_pos = 0.01f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err) * PI / 180.0f; // rad/s
- // L when P-gain = 100, f_cut = 10Hz
+ if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
+ temp_vel_pos = 0.1f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err) * PI / 180.0f; // rad/s
+ // L when P-gain = 100, f_cut = 10Hz
} else {
- temp_vel_pos = 0.01f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err); // mm/s
- // L when P-gain = 100, f_cut = 10Hz
+ temp_vel_pos = 0.1f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err); // mm/s
+ // L when P-gain = 100, f_cut = 10Hz
}
// torque control command ===============================================================================================================================================
float alpha_SpringDamper = 1.0f/(1.0f+TMR_FREQ_5k/(2.0f*PI*30.0f));
K_LPF = alpha_SpringDamper * K_LPF + (1.0f-alpha_SpringDamper) * K_SPRING;
D_LPF = alpha_SpringDamper * D_LPF + (1.0f-alpha_SpringDamper) * D_DAMPER;
-
- if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
- float torq_ref_act = torq.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : Nm
+
+ if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
+ float torq_ref_act = torq.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : Nm
torq.err = torq_ref_act - torq.sen;
torq.err_int += torq.err/((float)TMR_FREQ_5k);
- temp_vel_FT = 0.0001f * (P_GAIN_JOINT_TORQUE * torq.err + I_GAIN_JOINT_TORQUE * torq.err_int); // Nm >> rad/s
+ temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * torq.err + I_GAIN_JOINT_TORQUE * torq.err_int); // Nm >> rad/s
} else {
- float force_ref_act = force.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : N
+ float force_ref_act = force.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : N
force.err = force_ref_act - force.sen;
force.err_int += force.err/((float)TMR_FREQ_5k);
- temp_vel_FT = 0.0001f * (P_GAIN_JOINT_TORQUE * force.err + I_GAIN_JOINT_TORQUE * force.err_int); // N >> mm/s
+ temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * force.err + I_GAIN_JOINT_TORQUE * force.err_int); // N >> mm/s
}
-
- // velocity feedforward command ========================================================================================================================================
- if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
+
+
+ // velocity feedforward command ========================================================================================================================================
+ if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
temp_vel_ff = 0.01f * (float)VELOCITY_COMP_GAIN * vel.ref * PI / 180.0f; // rad/s
} else {
temp_vel_ff = 0.01f * (float)VELOCITY_COMP_GAIN * vel.ref; // mm/s
}
- // command integration =================================================================================================================================================
+ // command integration =================================================================================================================================================
temp_vel = (1.0f - alpha_trans) * temp_vel_pos + alpha_trans * temp_vel_FT + temp_vel_ff; // Position Control + Torque Control + Velocity Feedforward
- float Qact = 0.0f; // required flow rate
+ float Qact = 0.0f; // required flow rate
if( temp_vel > 0.0f ) {
Qact = temp_vel * ((float)PISTON_AREA_A * 0.00006f); // mm^3/sec >> LPM
I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f))))/C_d;
@@ -793,23 +793,29 @@
I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f))))/C_d;
}
+ float I_MAX = 10.0f; // Maximum Current : 10mA
+
// Anti-windup for FT
- if (I_GAIN_JOINT_TORQUE != 0.0f) {
- float I_MAX = 10.0f; // Maximum Current : 10mA
+// if (I_GAIN_JOINT_TORQUE != 0.0f) {
+ if (I_GAIN_JOINT_TORQUE > 0.001f) {
float Ka = 2.0f;
if (I_REF > I_MAX) {
float I_rem = I_REF - I_MAX;
I_REF = I_MAX;
- float temp_vel_rem = K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_A * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
-// torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check!
+ float temp_vel_rem = K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_A * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE);
} else if (I_REF < -I_MAX) {
double I_rem = I_REF - (-I_MAX);
I_REF = -I_MAX;
- float temp_vel_rem = K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_B * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
-/* torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check!*/
+ float temp_vel_rem = K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_B * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE);
}
+ } else {
+ if(I_REF > I_MAX) {
+ I_REF = I_MAX;
+ } else if (I_REF < -I_MAX) {
+ I_REF = -I_MAX;
+ }
}
break;
}
@@ -909,7 +915,7 @@
if (I_REF_fil > 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_PLUS*mA_PER_pulse; // unit: mA
else if (I_REF_fil < 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_MINUS*mA_PER_pulse; // unit: mA
else I_REF_fil_DZ = I_REF_fil + (double)(VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f*mA_PER_pulse; // unit: mA
-
+
I_ERR = I_REF_fil_DZ - (double)cur.sen;
I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f;
@@ -969,7 +975,7 @@
if (CUR_PWM_lin > 0) V_out = (float) (CUR_PWM_lin + 169.0f);
else if (CUR_PWM_lin < 0) V_out = (float) (CUR_PWM_lin - 174.0f);
else V_out = (float) (CUR_PWM_lin);
-
+
} else { //////////////////////////sw valve
// Output Voltage Linearization & Dead Zone Cancellation (Electrical dead-zone) by SW
if (V_out > 0 ) V_out = (V_out + 180.0f)/0.8588f;
@@ -1009,29 +1015,31 @@
////////////////////////////////////////////////////////////////////////////
////////////////////// Data transmission through CAN //////////////////////
- ////////////////////////////////////////////////////////////////////////////
-
+ ////////////////////////////////////////////////////////////////////////////
+
if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
-
+
// Position, Velocity, and Torque (ID:1200)
if (flag_data_request[0] == HIGH) {
if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
-// CAN_TX_POSITION_FT((int16_t) (PRES_A_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_A.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+// CAN_TX_POSITION_FT((int16_t) (PRES_B_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_B.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+
} else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
// CAN_TX_POSITION_FT((int16_t) (PRES_B_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_B.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+// CAN_TX_POSITION_FT((int16_t) (logging*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
}
}
// Valve Position (ID:1300)
if (flag_data_request[1] == HIGH) {
- CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
+ CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
}
// Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400)
if (flag_data_request[2] == HIGH) {
- CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (PRES_SUPPLY), (int16_t) (PRES_SUPPLY_NOM));
+ CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (PRES_SUPPLY), (int16_t) (PRES_SUPPLY_NOM));
}
TMR2_COUNT_CAN_TX = 0;