Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2
rainbow
Diff: main.cpp
- Revision:
- 230:2c3e5ecbe7e1
- Parent:
- 229:7a1c46b9b471
- Child:
- 231:e00e71ca3e80
diff -r 7a1c46b9b471 -r 2c3e5ecbe7e1 main.cpp --- 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;