Buyoun Cho
[Ver 1.0] The code was given by Seunghoon shin, used for hydraulic quadrupedal robot. Buyoun Cho will revise the code for Post-LIGHT (the robot name is not determined yet).
Diff: main.cpp
- Revision:
- 230:2c3e5ecbe7e1
- Parent:
- 229:7a1c46b9b471
--- a/main.cpp Tue Apr 20 00:42:45 2021 +0000
+++ b/main.cpp Wed Apr 21 04:20:39 2021 +0000
@@ -29,8 +29,8 @@
// dac & check ///////////////////////////////////////////
DigitalOut check(PC_2);
DigitalOut check_2(PC_3);
-AnalogOut dac_1(PA_4);
-AnalogOut dac_2(PA_5);
+AnalogOut dac_1(PA_4); // 0.0f ~ 1.0f
+AnalogOut dac_2(PA_5); // 0.0f ~ 1.0f
AnalogIn adc1(PC_4); //pressure_1
AnalogIn adc2(PB_0); //pressure_2
AnalogIn adc3(PC_1); //current
@@ -66,11 +66,12 @@
State pos;
State vel;
State Vout;
-State torq;
+State force;
+State torq; // unit : N
State torq_dot;
-State pres_A;
+State pres_A; // unit : bar
State pres_B;
-State cur;
+State cur; // unit : mA
State valve_pos;
State INIT_Vout;
@@ -91,26 +92,24 @@
extern int CID_TX_VALVE_POSITION;
extern int CID_TX_SOMETHING;
-float pres_A_new = 0.0f;
-float pres_B_new = 0.0f;
-
-
-
float temp_P_GAIN = 0.0f;
float temp_I_GAIN = 0.0f;
int temp_VELOCITY_COMP_GAIN = 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;
+ return log(sqrt((1.0f+y)/(1.0f-y)));
+}
+
/*******************************************************************************
* REFERENCE MODE
******************************************************************************/
enum _REFERENCE_MODE {
- MODE_REF_NO_ACT = 0, //0
- MODE_REF_DIRECT, //1
- MODE_REF_COS_INC, //2
- MODE_REF_LINE_INC, //3
- MODE_REF_SIN_WAVE, //4
- MODE_REF_SQUARE_WAVE, //5
+ MODE_REF_NO_ACT = 0,
+ MODE_REF_DIRECT,
+ MODE_REF_FINDHOME
};
/*******************************************************************************
@@ -234,7 +233,6 @@
make_delay();
-
// spi _ enc
spi_enc_set_init();
make_delay();
@@ -270,19 +268,14 @@
TIM3->CR1 ^= TIM_CR1_UDIS;
make_delay();
-
-
//Timer priority
NVIC_SetPriority(TIM3_IRQn, 2);
NVIC_SetPriority(TIM4_IRQn, 3);
-
-
-
//DAC init
if (SENSING_MODE == 0) {
- dac_1 = TORQUE_VREF / 3.3f;
+ dac_1 = FORCE_VREF / 3.3f;
dac_2 = 0.0f;
} else if (SENSING_MODE == 1) {
if (DIR_VALVE_ENC > 0) {
@@ -435,6 +428,9 @@
TIMER INTERRUPT
*******************************************************************************/
+//------------------------------------------------
+// TMR4 : Sensor Read & Data Handling
+//-----------------------------------------------
float FREQ_TMR4 = (float)FREQ_20k;
float DT_TMR4 = (float)DT_20k;
long CNT_TMR4 = 0;
@@ -442,52 +438,42 @@
extern "C" void TIM4_IRQHandler(void)
{
if (TIM4->SR & TIM_SR_UIF ) {
- /*******************************************************
- *** Sensor Read & Data Handling
- ********************************************************/
- //Encoder
+ // Current ===================================================
+ //ADC3->CR2 |= 0x40000000; // adc _ 12bit
+
+ cur.UpdateSen(((float)ADC3->DR-2047.5f)/2047.5f*10.0f, FREQ_TMR4, 500.0f); // unit : mA
+
+ // Encoder ===================================================
if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) {
ENC_UPDATE();
}
+ // Force or Pressure Transducer =============================================
ADC1->CR2 |= 0x40000000;
- // Torque Sensing =============================================
- if (SENSING_MODE == 0) {
- float pres_A_new = (((float) ADC1->DR) - 2047.5f);
- double alpha_update_ft = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 100.0f)); // f_cutoff : 100Hz
- pres_A.sen = (1.0f - alpha_update_ft) * pres_A.sen + alpha_update_ft * pres_A_new;
- torq.sen = -pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE;
-
- // Pressure Sensing (0~210)bar =============================================
- } else if (SENSING_MODE == 1) {
-
+ if (SENSING_MODE == 0) { // Force sensing
+ force.UpdateSen((((float)ADC1->DR) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR4, 100.0f); // unit : N
+ } else if (SENSING_MODE == 1) { // Pressure sensing
+ float pres_A_new, pres_B_new;
if (DIR_VALVE_ENC > 0) {
- pres_A_new = (((float)ADC1->DR) - PRES_A_NULL);
- pres_B_new = (((float)ADC2->DR) - PRES_B_NULL);
+ pres_A_new = (((float)ADC1->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
+ pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR;
} else {
- pres_A_new = (((float)ADC2->DR) - PRES_A_NULL);
- pres_B_new = (((float)ADC1->DR) - PRES_B_NULL);
+ pres_A_new = (((float)ADC2->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
+ pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR;
}
- double alpha_update_pres = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 200.0f)); // f_cutoff : 200Hz
- pres_A.sen = (1.0f - alpha_update_pres) * pres_A.sen + alpha_update_pres * pres_A_new;
- pres_B.sen = (1.0f - alpha_update_pres) * pres_B.sen + alpha_update_pres * pres_B_new;
- CUR_PRES_A_BAR = pres_A.sen / PRES_SENSOR_A_PULSE_PER_BAR;
- CUR_PRES_B_BAR = pres_B.sen / PRES_SENSOR_B_PULSE_PER_BAR;
+ pres_A.UpdateSen(pres_A_new,FREQ_TMR4,200.0f);
+ pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f);
if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
- torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 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.0); // unit : Nm
} else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
- torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.1f; // mm^2*bar >> N
+ float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N
+ force.UpdateSen(force_new,FREQ_TMR4,1000.0); // unit : N
}
}
- //Current
- //ADC3->CR2 |= 0x40000000; // adc _ 12bit
- float alpha_update_cur = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*500.0f)); // f_cutoff : 500Hz
- float cur_new = ((float)ADC3->DR-2048.0f)*20.0f/4096.0f; // unit : mA
- cur.sen=cur.sen*(1.0f-alpha_update_cur)+cur_new*(alpha_update_cur);
-
CNT_TMR4++;
}
TIM4->SR = 0x0; // reset the status register
@@ -505,26 +491,15 @@
{
if (TIM3->SR & TIM_SR_UIF ) {
-
- if (((OPERATING_MODE&0b110)>>1) == 0) {
- K_v = 0.4f; // Moog (LPM >> mA) , 100bar
- mV_PER_mA = 500.0f; // 5000mV/10mA
- mV_PER_pulse = 0.5f; // 5000mV/10000pulse
- mA_PER_pulse = 0.001f; // 10mA/10000pulse
- } else if (((OPERATING_MODE&0b110)>>1) == 1) {
- K_v = 0.5f; // KNR (LPM >> mA) , 100bar
- mV_PER_mA = 166.6666f; // 5000mV/30mA
- mV_PER_pulse = 0.5f; // 5000mV/10000pulse
- mA_PER_pulse = 0.003f; // 30mA/10000pulse
- }
-
if(MODE_POS_FT_TRANS == 1) {
+ if (alpha_trans == 1.0f) MODE_POS_FT_TRANS = 2;
alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
cnt_trans++;
torq.err_sum = 0;
if((float)cnt_trans * DT_TMR3 > 3.0f)
MODE_POS_FT_TRANS = 2;
} else if(MODE_POS_FT_TRANS == 3) {
+ if (alpha_trans == 0.0f) MODE_POS_FT_TRANS = 0;
alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
cnt_trans++;
torq.err_sum = 0;
@@ -539,22 +514,45 @@
}
-// Reference Loop ==========================================================
+ // Reference Update ==========================================================
switch (REFERENCE_MODE) {
case MODE_REF_NO_ACT: {
break;
}
-
case MODE_REF_DIRECT: {
pos.ref = REF_POSITION;
vel.ref = REF_VELOCITY;
torq.ref = REF_TORQUE;
+ force.ref = REF_FORCE;
+ break;
+ }
+ case MODE_REF_FINDHOME: {
+ pos.ref = REF_POSITION_FINDHOME;
+ vel.ref = 0.0f;
+ torq.ref = 0.0f;
+ force.ref = 0.0f;
break;
}
default:
break;
}
+ if (((OPERATING_MODE&0b010)>>1) == 0) {
+ K_v = 1.03f; // Q = K_v*sqrt(deltaP)*tanh(C_d*Xv);
+ K_v = 0.16f;
+ mV_PER_mA = 500.0f; // 5000mV/10mA
+ mV_PER_pulse = 0.5f; // 5000mV/10000pulse
+ mA_PER_pulse = 0.001f; // 10mA/10000pulse
+ } else if (((OPERATING_MODE&0b010)>>1) == 1) {
+ K_v = 0.5f; // KNR (LPM >> mA) , 100bar
+ mV_PER_mA = 166.6666f; // 5000mV/30mA
+ mV_PER_pulse = 0.5f; // 5000mV/10000pulse
+ mA_PER_pulse = 0.003f; // 30mA/10000pulse
+ }
+
+ // =====================================================================
+ // CONTROL LOOP --------------------------------------------------------
+ // =====================================================================
int UTILITY_MODE = 0;
int CONTROL_MODE = 0;
@@ -565,350 +563,145 @@
CONTROL_MODE = CONTROL_UTILITY_MODE;
UTILITY_MODE = MODE_NO_ACT;
}
-
-
-
// UTILITY MODE ------------------------------------------------------------
switch (UTILITY_MODE) {
case MODE_NO_ACT: {
break;
}
- case MODE_TORQUE_SENSOR_NULLING: {
+ 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
- if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
- CUR_TORQUE_sum += torq.sen;
+ float VREF_TuningGain = 0.000003f;
+ if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 5) {
+ LED = 1;
+ if(SENSING_MODE == 0) { // Force Sensor (Loadcell)
+ FORCE_pulse_sum = FORCE_pulse_sum + force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE;
+ if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
+ float FORCE_pluse_mean = FORCE_pulse_sum / 10.0f;
+ FORCE_pulse_sum = 0.0f;
- if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
- CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0f;
- CUR_TORQUE_sum = 0;
-
- TORQUE_VREF += 0.000003f * (0.0f - CUR_TORQUE_mean);
+ FORCE_VREF += VREF_TuningGain * (0.0f - FORCE_pluse_mean);
+ if (FORCE_VREF > 3.3f) FORCE_VREF = 3.3f;
+ if (FORCE_VREF < 0.0f) FORCE_VREF = 0.0f;
+ dac_1 = FORCE_VREF / 3.3f;
+ }
+ } 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) {
+ float PresA_pluse_mean = PresA_pulse_sum / 10.0f;
+ float PresB_pluse_mean = PresB_pulse_sum / 10.0f;
+ PresA_pulse_sum = 0.0f;
+ PresB_pulse_sum = 0.0f;
- if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f;
- if (TORQUE_VREF < 0.0f) TORQUE_VREF = 0.0f;
- dac_1 = TORQUE_VREF / 3.3f;
+ PRES_A_VREF += VREF_TuningGain * (0.0f - PresA_pluse_mean);
+ if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
+ if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
+ 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;
+ dac_2 = PRES_B_VREF / 3.3f;
+ }
}
+ TMR3_COUNT_TORQUE_NULL++;
} else {
+ if(SENSING_MODE == 0 ) { // Force Sensor (Loadcell)
+ FORCE_pulse_sum = 0.0f;
+ dac_1 = FORCE_VREF / 3.3f;
+ spi_eeprom_write(RID_FORCE_SENSOR_VREF, (int16_t)(FORCE_VREF * 1000.0f));
+ } else if (SENSING_MODE == 1) {
+ PresA_pulse_sum = 0.0f;
+ PresB_pulse_sum = 0.0f;
+ dac_1 = PRES_A_VREF / 3.3f;
+ dac_2 = PRES_B_VREF / 3.3f;
+ spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t)(PRES_A_VREF * 1000.0f));
+ spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t)(PRES_B_VREF * 1000.0f));
+ }
CONTROL_UTILITY_MODE = MODE_NO_ACT;
TMR3_COUNT_TORQUE_NULL = 0;
- CUR_TORQUE_sum = 0;
- CUR_TORQUE_mean = 0;
-
- spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0f));
-
- dac_1 = TORQUE_VREF / 3.3f;
-
}
- TMR3_COUNT_TORQUE_NULL++;
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;
+
if (FINDHOME_STAGE == FINDHOME_INIT) {
- REFERENCE_MODE=MODE_REF_NO_ACT;
+ REFERENCE_MODE = MODE_REF_FINDHOME;
cnt_findhome = 0;
- cnt_vel_findhome = 0;
+ cnt_terminate_findhome = 0;
pos.ref = pos.sen;
vel.ref = 0.0f;
+ REF_POSITION_FINDHOME = pos.ref;
FINDHOME_STAGE = FINDHOME_GOTOLIMIT;
} else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) {
- int cnt_check_enc = (TMR_FREQ_5k/20);
+ int cnt_check_enc = (TMR_FREQ_5k/20); // 5000/20 = 250tic = 50msec
if(cnt_findhome%cnt_check_enc == 0) {
- FINDHOME_POSITION = pos.sen;
- FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD;
- FINDHOME_POSITION_OLD = FINDHOME_POSITION;
+ FINDHOME_POSITION_pulse = pos.sen*ENC_PULSE_PER_POSITION;
+ FINDHOME_VELOCITY_pulse = FINDHOME_POSITION_pulse - FINDHOME_POSITION_pulse_OLD;
+ FINDHOME_POSITION_pulse_OLD = FINDHOME_POSITION_pulse;
}
cnt_findhome++;
- if (abs(FINDHOME_VELOCITY) <= 1) {
- cnt_vel_findhome = cnt_vel_findhome + 1;
+ if (fabs(FINDHOME_VELOCITY_pulse) <= 1) {
+ cnt_terminate_findhome = cnt_terminate_findhome + 1;
} else {
- cnt_vel_findhome = 0;
+ cnt_terminate_findhome = 0;
}
- if ((cnt_vel_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec
+ 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) {
- pos.ref = pos.ref + 12.0f;
+ REF_POSITION_FINDHOME = REF_POSITION_FINDHOME + GOTOHOME_SPEED*DT_5k;
} else {
- pos.ref = pos.ref - 12.0f;
+ REF_POSITION_FINDHOME = REF_POSITION_FINDHOME - GOTOHOME_SPEED*DT_5k;
}
-
-
CONTROL_MODE = MODE_JOINT_CONTROL;
alpha_trans = 0.0f;
-
} else {
- ENC_SET((int32_t)((int32_t)HOMEPOS_OFFSET*10));
- INIT_REF_POS = (int32_t)((int32_t)HOMEPOS_OFFSET*10);
- REF_POSITION = 0;
- REF_VELOCITY = 0;
- FINDHOME_POSITION = 0;
- FINDHOME_POSITION_OLD = 0;
- FINDHOME_VELOCITY = 0;
+ ENC_SET((long)((long)HOMEPOS_OFFSET*10));
+ REF_POSITION_FINDHOME_INIT = (float)((long)HOMEPOS_OFFSET*10);
+ FINDHOME_POSITION_pulse = 0;
+ FINDHOME_POSITION_pulse_OLD = 0;
+ FINDHOME_VELOCITY_pulse = 0;
+
cnt_findhome = 0;
- cnt_vel_findhome = 0;
+ cnt_terminate_findhome = 0;
+ pos.ref = 0.0f;
FINDHOME_STAGE = FINDHOME_ZEROPOSE;
-
-
- cnt_findhome = 0;
- pos.ref = 0.0f;
- vel.ref = 0.0f;
- pos.ref_home_pos = 0.0f;
- vel.ref_home_pos = 0.0f;
}
} else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) {
+
int T_move = 2*TMR_FREQ_5k;
- pos.ref = (0.0f - (float)INIT_REF_POS)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)INIT_REF_POS;
- vel.ref = 0.0f;
- alpha_trans = 0.0f;
-
- pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
- vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
- pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
- CONTROL_MODE = MODE_JOINT_CONTROL;
+ 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;
cnt_findhome++;
if (cnt_findhome >= T_move) {
cnt_findhome = 0;
pos.ref = 0.0f;
- vel.ref = 0.0f;
- pos.ref_home_pos = 0.0f;
- vel.ref_home_pos = 0.0f;
FINDHOME_STAGE = FINDHOME_INIT;
CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
- REFERENCE_MODE=MODE_REF_DIRECT;
- }
- }
-
- break;
- }
-
- case MODE_PRESSURE_SENSOR_NULLING: {
- // DAC Voltage reference set
- if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
- CUR_PRES_A_sum += pres_A.sen;
- CUR_PRES_B_sum += pres_B.sen;
-
- if (TMR3_COUNT_PRES_NULL % 10 == 0) {
- CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0f;
- CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0f;
- CUR_PRES_A_sum = 0;
- CUR_PRES_B_sum = 0;
-
- float VREF_NullingGain = 0.0003f;
- PRES_A_VREF = PRES_A_VREF + VREF_NullingGain * CUR_PRES_A_mean;
- PRES_B_VREF = PRES_B_VREF + VREF_NullingGain * CUR_PRES_B_mean;
-
- if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
- if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
- if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f;
- if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
-
- if (DIR_VALVE_ENC > 0) {
- dac_1 = PRES_A_VREF / 3.3f;
- dac_2 = PRES_B_VREF / 3.3f;
- } else {
- dac_1 = PRES_B_VREF / 3.3f;
- dac_2 = PRES_A_VREF / 3.3f;
- }
- }
- } else {
- CONTROL_UTILITY_MODE = MODE_NO_ACT;
- TMR3_COUNT_PRES_NULL = 0;
- CUR_PRES_A_sum = 0;
- CUR_PRES_B_sum = 0;
- CUR_PRES_A_mean = 0;
- CUR_PRES_B_mean = 0;
-
- spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0f));
- spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0f));
-
- if (DIR_VALVE_ENC > 0) {
- dac_1 = PRES_A_VREF / 3.3f;
- dac_2 = PRES_B_VREF / 3.3f;
- } else {
- dac_1 = PRES_B_VREF / 3.3f;
- dac_2 = PRES_A_VREF / 3.3f;
+ REFERENCE_MODE = MODE_REF_DIRECT;
}
}
- TMR3_COUNT_PRES_NULL++;
- break;
- }
-
- case MODE_DDV_DEADZONE_AND_CENTER: {
- if (FINDDZ_STAGE == FINDDZ_INIT) {
- cnt_finddz = 0;
- cnt_vel_finddz = 0;
- temp_pos_ref = pos.sen;
- temp_pos_ref_offset = 0.0f;
- vel.ref = 0.0f;
- FINDDZ_STAGE = FINDDZ_START1;
- flag_finddz = 1;
- FINDDZ_POSITION = pos.sen;
- FINDDZ_POSITION_OLD = FINDDZ_POSITION;
-
- temp_P_GAIN = P_GAIN_JOINT_POSITION;
- temp_I_GAIN = I_GAIN_JOINT_POSITION;
- temp_VELOCITY_COMP_GAIN = VELOCITY_COMP_GAIN;
-
- P_GAIN_JOINT_POSITION = 1.0f;
- I_GAIN_JOINT_POSITION = 0.0f;
- VELOCITY_COMP_GAIN = 0;
- FLAG_VALVE_DEADZONE = 0;
- alpha_trans = 0.0f;
- REFERENCE_MODE=MODE_REF_NO_ACT;
-
- } else if (FINDDZ_STAGE == FINDDZ_START1) {
- cnt_finddz++;
- CONTROL_MODE = MODE_JOINT_CONTROL;
- FINDDZ_POSITION = pos.sen;
- FINDDZ_VELOCITY = FINDDZ_POSITION - FINDDZ_POSITION_OLD;
- FINDDZ_POSITION_OLD = FINDDZ_POSITION;
-
- if (temp_P_GAIN > 0.0f) temp_pos_ref_offset = temp_pos_ref_offset - (float)FINDDZ_VELOCITY*temp_P_GAIN;
- else temp_pos_ref_offset = temp_pos_ref_offset - (float)FINDDZ_VELOCITY*20.0f;
-
- pos.ref = temp_pos_ref + temp_pos_ref_offset;
-
- if (cnt_finddz > 10000) FINDDZ_STAGE = FINDDZ_START2;
-
-
- } else if (FINDDZ_STAGE == FINDDZ_START2) {
- CONTROL_MODE = MODE_JOINT_CONTROL;
- FINDDZ_POSITION = pos.sen;
- FINDDZ_VELOCITY = FINDDZ_POSITION - FINDDZ_POSITION_OLD;
- FINDDZ_POSITION_OLD = FINDDZ_POSITION;
-
- if (flag_finddz > 0) {
- temp_pos_ref = temp_pos_ref + 1.0f;
- if (FINDDZ_VELOCITY > 0) {
- cnt_vel_finddz = cnt_vel_finddz + 1;
- }
- if (cnt_vel_finddz > 5) {
- VALVE_DEADZONE_PLUS = (int)(I_REF_fil/mA_PER_pulse);
-// temp_VALVE_DEADZONE_PLUS = temp_VALVE_DEADZONE_PLUS + I_REF_fil/mA_PER_pulse;
- cnt_vel_finddz = 0;
- flag_finddz = - flag_finddz - 1;
- }
-
- } else if (flag_finddz < 0) {
- temp_pos_ref = temp_pos_ref - 1.0f;
- if (FINDDZ_VELOCITY < 0) {
- cnt_vel_finddz = cnt_vel_finddz + 1;
- }
- if (cnt_vel_finddz > 5) {
- VALVE_DEADZONE_MINUS = (int)(I_REF_fil/mA_PER_pulse);
-// temp_VALVE_DEADZONE_MINUS = temp_VALVE_DEADZONE_MINUS + I_REF_fil/mA_PER_pulse;
- cnt_vel_finddz = 0;
- flag_finddz = - flag_finddz + 1;
- }
- }
- if (abs(flag_finddz) >= 6) {
- FINDDZ_STAGE = FINDDZ_STOP;
-// VALVE_DEADZONE_PLUS = (int)(temp_VALVE_DEADZONE_PLUS / (double)flag_finddz * 2.0f);
-// VALVE_DEADZONE_MINUS = (int)(temp_VALVE_DEADZONE_MINUS / (double)flag_finddz * 2.0f);
- spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
- spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
- }
- pos.ref = temp_pos_ref + temp_pos_ref_offset;
-
- } else if (FINDDZ_STAGE == FINDDZ_STOP) {
- FINDDZ_STAGE = FINDDZ_INIT;
- CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
- P_GAIN_JOINT_POSITION = temp_P_GAIN;
- I_GAIN_JOINT_POSITION = temp_I_GAIN;
- VELOCITY_COMP_GAIN = temp_VELOCITY_COMP_GAIN;
- flag_finddz = 0;
- FLAG_VALVE_DEADZONE = 1;
- REFERENCE_MODE=MODE_REF_DIRECT;
- }
break;
}
-
- case MODE_SYSTEM_ID: {
- freq_sysid_Iref = (double) cnt_sysid * DT_TMR3 * 3.0f;
- valve_pos.ref = 2500.0f * sin(2.0f * 3.14159f * freq_sysid_Iref * (double) cnt_sysid * DT_TMR3);
- CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
- cnt_sysid++;
- if (freq_sysid_Iref >= 300) {
- cnt_sysid = 0;
- CONTROL_UTILITY_MODE = MODE_NO_ACT;
- }
- break;
- }
-
- case MODE_FREQ_TEST: {
- float valve_pos_ref = 2500.0f * sin(2.0f * 3.141592f * freq_test_valve_ref * (float) cnt_freq_test * DT_TMR3);
- if(valve_pos_ref >= 0) {
- valve_pos.ref = (double)VALVE_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_CENTER)/10000.0f;
- } else {
- valve_pos.ref = (double)VALVE_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_CENTER)/10000.0f;
- }
- ref_array[cnt_freq_test] = valve_pos_ref;
- if(value>=(float) VALVE_CENTER) {
- pos_array[cnt_freq_test] = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
- } else {
- pos_array[cnt_freq_test] = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
- }
-
- CONTROL_MODE = MODE_VALVE_POSITION_CONTROL;
- cnt_freq_test++;
- if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR3 > 2) {
- buffer_data_size = cnt_freq_test;
- cnt_freq_test = 0;
- cnt_send_buffer = 0;
- freq_test_valve_ref = freq_test_valve_ref * 1.05f;
- if (freq_test_valve_ref >= 400) {
- CONTROL_UTILITY_MODE = MODE_NO_ACT;
- CONTROL_MODE = MODE_NO_ACT;
- CAN_TX_PWM((int16_t) (1)); //1300
- }
- CONTROL_MODE = MODE_NO_ACT;
- CONTROL_UTILITY_MODE = MODE_SEND_OVER;
-
- }
- break;
- }
-
- case MODE_STEP_TEST: {
- float valve_pos_ref = 0.0f;
- if (cnt_step_test < (int) (1.0f * (float) TMR_FREQ_5k)) {
- valve_pos_ref = 0.0f;
- } else {
- valve_pos_ref = 10000.0f;
- }
- if(valve_pos_ref >= 0) {
- valve_pos.ref = (double)VALVE_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_CENTER)/10000.0f;
- } else {
- valve_pos.ref = (double)VALVE_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_CENTER)/10000.0f;
- }
- ref_array[cnt_step_test] = valve_pos_ref;
- if(value>=(float) VALVE_CENTER) {
- pos_array[cnt_step_test] = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
- } else {
- pos_array[cnt_step_test] = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
- }
-
- CONTROL_MODE = MODE_VALVE_POSITION_CONTROL;
- cnt_step_test++;
- if (cnt_step_test > (int) (2.0f * (float) TMR_FREQ_5k)) {
- buffer_data_size = cnt_step_test;
- cnt_step_test = 0;
- cnt_send_buffer = 0;
- CONTROL_UTILITY_MODE = MODE_SEND_OVER;
- CONTROL_MODE = MODE_NO_ACT;
- }
-
- break;
- }
-
default:
break;
}
-
-
// CONTROL MODE ------------------------------------------------------------
switch (CONTROL_MODE) {
case MODE_NO_ACT: {
@@ -921,125 +714,93 @@
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;
+ I_REF = valve_pos.ref * 0.001f; // Unit : pulse >> mA
+ float I_MAX = 10.0f; // Max : 10mA
+ if (I_REF > I_MAX) {
+ I_REF = I_MAX;
+ } else if (I_REF < -I_MAX) {
+ I_REF = -I_MAX;
+ }
}
break;
}
- case MODE_JOINT_CONTROL: {
-
- double torq_ref = 0.0f;
- pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
- vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
- pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
-
- //K & D Low Pass Filter
- float alpha_K_D = 1.0f/(1.0f + 5000.0f/(2.0f*3.14f*30.0f)); // f_cutoff : 30Hz
- K_LPF = K_LPF*(1.0f-alpha_K_D)+K_SPRING*(alpha_K_D);
- D_LPF = D_LPF*(1.0f-alpha_K_D)+D_DAMPER*(alpha_K_D);
-
- torq_ref = torq.ref + K_LPF * pos.err - D_LPF * vel.sen / ENC_PULSE_PER_POSITION; //[N]
+ case MODE_JOINT_CONTROL:
+ {
- // torque feedback
- torq.err = torq_ref - torq.sen; //[N]
- torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
-
- if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
-
- double I_REF_POS = 0.0f;
- double I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
- double I_REF_VC = 0.0f; // I_REF for velocity compensation
-
- double temp_vel_pos = 0.0f;
- double temp_vel_torq = 0.0f;
- double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position 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 = 0.0f;
- if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
- temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s
- // L when P-gain = 100, f_cut = 10Hz L feedforward velocity
- } else if ((OPERATING_MODE & 0x01) == 1) {
- temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s
- // L when P-gain = 100, f_cut = 10Hz L feedforward velocity
- }
- if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
- else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
-
- // velocity compensation for torque control
- if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
- I_REF_FORCE_FB = 0.001f * ((double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum);
- // temp_vel_torq = (0.01 * (double) VELOCITY_COMP_GAIN * (double) CUR_VELOCITY / (double) ENC_PULSE_PER_POSITION) * PI / 180.0; // rad/s
- temp_vel_torq = (0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / (double) ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s
- // L feedforward velocity
- } else if ((OPERATING_MODE & 0x01) == 1) {
- I_REF_FORCE_FB = 0.001f * 0.01f*((double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum); // Linear Actuators are more sensitive.
- // temp_vel_torq = (0.01 * (double) VELOCITY_COMP_GAIN * (double) CUR_VELOCITY / (double) ENC_PULSE_PER_POSITION); // mm/s
- temp_vel_torq = (0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / (double) ENC_PULSE_PER_POSITION); // mm/s
- // L feedforward velocity
- }
- if (temp_vel_torq > 0.0f) I_REF_VC = temp_vel_torq * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
- else I_REF_VC = temp_vel_torq * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
- // L velocity(rad/s or mm/s) >> I_ref(mA)
- // Ref_Joint_FT_dot = (Ref_Joint_FT_Nm - Ref_Joint_FT_Nm_old) / TMR_DT_5k;
- // Ref_Joint_FT_Nm_old = Ref_Joint_FT_Nm;
+ 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
+ } else {
+ temp_vel_pos = 0.01f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err); // mm/s
+ // L when P-gain = 100, f_cut = 10Hz
+ }
- I_REF = (1.0f - alpha_trans) * I_REF_POS + alpha_trans * (I_REF_VC + I_REF_FORCE_FB);
-
- // Anti-windup for FT
- if (I_GAIN_JOINT_TORQUE != 0) {
- double I_MAX = 10.0f; // Maximum Current : 10mV
- double Ka = 2.0f / ((double) I_GAIN_JOINT_TORQUE * 0.001f);
- if (I_REF > I_MAX) {
- double I_rem = I_REF - I_MAX;
- I_rem = Ka*I_rem;
- I_REF = I_MAX;
- torq.err_sum = torq.err_sum - I_rem /(float) TMR_FREQ_5k;
- } else if (I_REF < -I_MAX) {
- double I_rem = I_REF - (-I_MAX);
- I_rem = Ka*I_rem;
- I_REF = -I_MAX;
- torq.err_sum = torq.err_sum - I_rem /(float) TMR_FREQ_5k;
- }
- }
-
+ // 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
+ 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
} else {
- float VALVE_POS_RAW_FORCE_FB = 0.0f;
- float VALVE_POS_RAW_FORCE_FF = 0.0f;
- float VALVE_POS_RAW = 0.0f;
-
- VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * (torq.ref_diff - torq_dot.sen)) * 0.01f + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref));
-
- VALVE_POS_RAW_FORCE_FF = P_GAIN_JOINT_TORQUE_FF * torq_ref * 0.001f + D_GAIN_JOINT_TORQUE_FF * (torq_ref - torq_ref_past) * 0.0001f;
+ 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
+ }
+
+ // 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
+ }
- VALVE_POS_RAW = VALVE_POS_RAW_FORCE_FB + VALVE_POS_RAW_FORCE_FF;
-
-
- if (VALVE_POS_RAW >= 0) {
- valve_pos.ref = VALVE_POS_RAW + VALVE_DEADZONE_PLUS;
- } else {
- valve_pos.ref = VALVE_POS_RAW + VALVE_DEADZONE_MINUS;
+ // 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
+ 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;
+ } else {
+ Qact = temp_vel * ((float)PISTON_AREA_B * 0.00006f); // mm^3/sec >> LPM
+ I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f))))/C_d;
+ }
+
+ // Anti-windup for FT
+ if (I_GAIN_JOINT_TORQUE != 0.0f) {
+ float I_MAX = 10.0f; // Maximum Current : 10mA
+ 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!
+ } 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!
}
-
- if(I_GAIN_JOINT_TORQUE != 0) {
- double Ka = 2.0f / (double) I_GAIN_JOINT_TORQUE * 100.0f;
- if(valve_pos.ref>VALVE_MAX_POS) {
- double valve_pos_rem = valve_pos.ref - VALVE_MAX_POS;
- valve_pos_rem = valve_pos_rem * Ka;
- valve_pos.ref = VALVE_MAX_POS;
- torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k;
- } else if(valve_pos.ref < VALVE_MIN_POS) {
- double valve_pos_rem = valve_pos.ref - VALVE_MIN_POS;
- valve_pos_rem = valve_pos_rem * Ka;
- valve_pos.ref = VALVE_MIN_POS;
- torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k;
- }
- }
-
- VALVE_POS_CONTROL(valve_pos.ref);
- V_out = (float) Vout.ref;
}
- torq_ref_past = torq_ref;
break;
}
@@ -1048,79 +809,78 @@
break;
}
- case MODE_JOINT_ADAPTIVE_BACKSTEPPING: {
-
-
- float Va = (1256.6f + Amm * pos.sen/(float)(ENC_PULSE_PER_POSITION)) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x, unit : m^3
- float Vb = (1256.6f + Amm * (79.0f - pos.sen/(float)(ENC_PULSE_PER_POSITION))) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x), unit : m^3
-
- V_adapt = 1.0f / (1.0f/Va + 1.0f/Vb); //initial 0.0000053f
-
- //float f3 = -Amm*Amm*beta*0.000001f*0.000001f/V_adapt * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076
- float f3_hat = -a_hat * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076
-
- float g3_prime = 0.0f;
- if (torq.sen > Amm*(Ps-Pt)*0.000001f) {
- g3_prime = 1.0f;
- } else if (torq.sen < -Amm*(Ps-Pt)*0.000001f) {
- g3_prime = -1.0f;
- } else {
- if ((value-VALVE_CENTER) > 0) {
- g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f);
-// g3_prime = sqrt(Ps-Pt);
- } else {
- g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f);
-// g3_prime = sqrt(Ps-Pt);
- }
- }
- float tau = 0.01f;
- float K_valve = 0.0004f;
-
- float x_v = 0.0f; //x_v : -1~1
- if(value>=VALVE_CENTER) {
- x_v = 1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
- } else {
- x_v = -1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
- }
- float f4 = -x_v/tau;
- float g4 = K_valve/tau;
-
- float torq_ref_dot = torq.ref_diff * 500.0f;
-
- pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
- vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
- pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
-
- torq.err = torq.ref - torq.sen; //[N]
- torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
-
- float k3 = 2000.0f; //2000 //20000
- float k4 = 10.0f;
- float rho3 = 3.2f;
- float rho4 = 10000000.0f; //25000000.0f;
- float x_4_des = (-f3_hat + torq_ref_dot - k3*(-torq.err))/(gamma_hat*g3_prime);
- if (x_4_des > 1) x_4_des = 1;
- else if (x_4_des < -1) x_4_des = -1;
-
- if (x_4_des > 0) {
- valve_pos.ref = x_4_des * (float)(VALVE_MAX_POS - VALVE_CENTER) + (float) VALVE_CENTER;
- } else {
- valve_pos.ref = x_4_des * (float)(VALVE_CENTER - VALVE_MIN_POS) + (float) VALVE_CENTER;
- }
-
- float x_4_des_dot = (x_4_des - x_4_des_old)*(float) TMR_FREQ_5k;
- x_4_des_old = x_4_des;
- V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*(-torq.err))/g4;
-
- float rho_a = 0.00001f;
- float a_hat_dot = -rho3/rho_a*vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f*(-torq.err);
- a_hat = a_hat + a_hat_dot / (float) TMR_FREQ_5k;
-
- if(a_hat > -3000000.0f) a_hat = -3000000.0f;
- else if(a_hat < -30000000.0f) a_hat = -30000000.0f;
-
- break;
- }
+// case MODE_JOINT_ADAPTIVE_BACKSTEPPING: {
+//
+// float Va = (1256.6f + Amm * pos.sen/(float)(ENC_PULSE_PER_POSITION)) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x, unit : m^3
+// float Vb = (1256.6f + Amm * (79.0f - pos.sen/(float)(ENC_PULSE_PER_POSITION))) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x), unit : m^3
+//
+// V_adapt = 1.0f / (1.0f/Va + 1.0f/Vb); //initial 0.0000053f
+//
+// //float f3 = -Amm*Amm*beta*0.000001f*0.000001f/V_adapt * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076
+// float f3_hat = -a_hat * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076
+//
+// float g3_prime = 0.0f;
+// if (torq.sen > Amm*(Ps-Pt)*0.000001f) {
+// g3_prime = 1.0f;
+// } else if (torq.sen < -Amm*(Ps-Pt)*0.000001f) {
+// g3_prime = -1.0f;
+// } else {
+// if ((value-VALVE_CENTER) > 0) {
+// g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f);
+//// g3_prime = sqrt(Ps-Pt);
+// } else {
+// g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f);
+//// g3_prime = sqrt(Ps-Pt);
+// }
+// }
+// float tau = 0.01f;
+// float K_valve = 0.0004f;
+//
+// float x_v = 0.0f; //x_v : -1~1
+// if(value>=VALVE_CENTER) {
+// x_v = 1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+// } else {
+// x_v = -1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+// }
+// float f4 = -x_v/tau;
+// float g4 = K_valve/tau;
+//
+// float torq_ref_dot = torq.ref_diff * 500.0f;
+//
+// pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
+// vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
+// pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
+//
+// torq.err = torq.ref - torq.sen; //[N]
+// torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
+//
+// float k3 = 2000.0f; //2000 //20000
+// float k4 = 10.0f;
+// float rho3 = 3.2f;
+// float rho4 = 10000000.0f; //25000000.0f;
+// float x_4_des = (-f3_hat + torq_ref_dot - k3*(-torq.err))/(gamma_hat*g3_prime);
+// if (x_4_des > 1) x_4_des = 1;
+// else if (x_4_des < -1) x_4_des = -1;
+//
+// if (x_4_des > 0) {
+// valve_pos.ref = x_4_des * (float)(VALVE_MAX_POS - VALVE_CENTER) + (float) VALVE_CENTER;
+// } else {
+// valve_pos.ref = x_4_des * (float)(VALVE_CENTER - VALVE_MIN_POS) + (float) VALVE_CENTER;
+// }
+//
+// float x_4_des_dot = (x_4_des - x_4_des_old)*(float) TMR_FREQ_5k;
+// x_4_des_old = x_4_des;
+// V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*(-torq.err))/g4;
+//
+// float rho_a = 0.00001f;
+// float a_hat_dot = -rho3/rho_a*vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f*(-torq.err);
+// a_hat = a_hat + a_hat_dot / (float) TMR_FREQ_5k;
+//
+// if(a_hat > -3000000.0f) a_hat = -3000000.0f;
+// else if(a_hat < -30000000.0f) a_hat = -30000000.0f;
+//
+// break;
+// }
default:
break;
@@ -1136,13 +896,10 @@
double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz
I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF;
- if (FLAG_VALVE_DEADZONE) {
- 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
- } else {
- I_REF_fil_DZ = I_REF_fil;
- }
+ 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;
@@ -1184,10 +941,8 @@
VALVE_PWM_RAW = -V_MAX;
I_ERR_INT = I_ERR_INT - V_rem * 0.0002f;
}
- Cur_Valve_Open_pulse = cur.sen / mA_PER_pulse;
} else {
VALVE_PWM_RAW = I_REF * mV_PER_mA;
- Cur_Valve_Open_pulse = I_REF / mA_PER_pulse;
}
////////////////////////////////////////////////////////////////////////////
@@ -1202,6 +957,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;
@@ -1209,9 +965,9 @@
else V_out = 0.0f;
}
- /*******************************************************
- *** PWM
- ********************************************************/
+ ////////////////////////////////////////////////////////////////////
+ /////////////////// PWM Command ///////////////////////////////////
+ ////////////////////////////////////////////////////////////////////
if(DIR_VALVE<0) {
V_out = -V_out;
}
@@ -1239,38 +995,29 @@
TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
-
+ ////////////////////////////////////////////////////////////////////////////
+ ////////////////////// 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
- if (SENSING_MODE == 0) {
- CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
- } else if (SENSING_MODE == 1) {
- CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
- }
+ CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.1f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
} else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
- if (SENSING_MODE == 0) {
- CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
- } else if (SENSING_MODE == 1) {
- CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
- }
+ CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION*0.25f), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.01f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
}
}
- // PWM, ID:1300
+ // Valve Position (ID:1300)
if (flag_data_request[1] == HIGH) {
- CAN_TX_PWM((int16_t) (V_out)); //1300
+ CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
}
- // Reference Current, Current Current (ID:1400)
+ // Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400)
if (flag_data_request[2] == HIGH) {
- CAN_TX_CURRENT((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse)); // 1400
- }
-
- // Reference Current, Current Current (ID:1700)
- if (flag_data_request[3] == HIGH) {
- CAN_TX_SOMETHING((int16_t) (0), (int16_t) (0), (int16_t) (0), (int16_t) (0)); // 1700
+ 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;