![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
Diff: main.cpp
- Revision:
- 45:35fa6884d0c6
- Parent:
- 44:fe7d5cfd2eea
- Child:
- 46:2694daea349b
diff -r fe7d5cfd2eea -r 35fa6884d0c6 main.cpp --- a/main.cpp Tue Jan 14 09:11:20 2020 +0000 +++ b/main.cpp Thu Jan 16 12:05:15 2020 +0000 @@ -76,11 +76,7 @@ extern int CID_RX_CMD; extern int CID_RX_REF_POSITION; -extern int CID_RX_REF_TORQUE; -extern int CID_RX_REF_PRES_DIFF; -extern int CID_RX_REF_VOUT; -extern int CID_RX_REF_VALVE_POSITION; -extern int CID_RX_REF_CURRENT; +extern int CID_RX_REF_PWM; extern int CID_TX_INFO; extern int CID_TX_POSITION; @@ -114,7 +110,7 @@ MODE_VALVE_OPEN_LOOP, //1 MODE_VALVE_POSITION_CONTROL, //2 - MODE_JOINT_POSITION_TORQUE_CONTROL_PWM, //3 + MODE_JOINT_CONTROL, //3 MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION, //4 MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5 @@ -269,9 +265,7 @@ Ref_Valve_Pos_FF = (float) VALVE_MIN_POS; } - //VELOCITY_COMP_GAIN = 20; - //CAN_TX_PRES((int16_t)(Ref_Valve_Pos_FF), (int16_t) (7)); - Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER) + DDV_CENTER; + Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER); return Ref_Valve_Pos_FF; } @@ -450,10 +444,34 @@ float FREQ_TMR3 = (float)FREQ_1k; //float DT_TMR3 = (float)DT_5k; float DT_TMR3 = (float)DT_1k; +int cnt_trans = 0; extern "C" void TIM3_IRQHandler(void) { if (TIM3->SR & TIM_SR_UIF ) { ENC_UPDATE(); + + if(MODE_POS_FT_TRANS == 1){ + alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans/(float)(3*TMR_FREQ_5k)))/2.0f; + cnt_trans++; + if(cnt_trans>3*TMR_FREQ_5k) + MODE_POS_FT_TRANS = 2; + } + if(MODE_POS_FT_TRANS == 3){ + alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans/(float)(3*TMR_FREQ_5k)))/2.0f; + cnt_trans++; + if(cnt_trans>3*TMR_FREQ_5k) + MODE_POS_FT_TRANS = 0; + } + else if(MODE_POS_FT_TRANS == 2){ + alpha_trans = 1.0; + cnt_trans = 0; + } + else{ + alpha_trans = 0.0; + cnt_trans = 0; + } + + // CONTROL LOOP ------------------------------------------------------------ @@ -473,42 +491,31 @@ break; } - case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: { - float PWM_RAW_POS_FB = 0.0f; // PWM by Position Feedback - float PWM_RAW_POS_FF = 0.0f; // PWM by Position Feedforward - float PWM_RAW_FORCE_FB = 0.0f; // PWM by Force Feedback - - // feedback input for position control - pos.err = pos.ref - (float) pos.sen; - pos.err_diff = pos.err - pos.err_old; - pos.err_old = pos.err; - pos.err_sum += pos.err; - if (pos.err_sum > 1000) pos.err_sum = 1000; - if (pos.err_sum<-1000) pos.err_sum = -1000; - // PWM_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff; - PWM_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err; - PWM_RAW_POS_FB = PWM_RAW_POS_FB * 0.01f; + case MODE_JOINT_CONTROL: { + + float VALVE_POS_RAW_FORCE_FB = 0.0f; + + pos.err = pos.ref - pos.sen; //[pulse] + vel.err = vel.ref - vel.sen; //[pulse/s] + double K_spring = 1.0f; //[N/mm] + double D_damper = 0.0001f; + torq.ref = torq.ref + (K_spring * pos.err + D_damper * vel.err) / ENC_PULSE_PER_POSITION; //[N] + + // torque feedback + torq.err = torq.ref - torq.sen; //[pulse] + torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse] + if (torq.err_sum > 1000) torq.err_sum = 1000; + if (torq.err_sum<-1000) torq.err_sum = -1000; + - // feedforward input for position control - float Ref_Vel_Act = vel.ref/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s] - float K_ff = 0.9f; - if(Ref_Vel_Act > 0) K_ff = 0.90f; // open - if(Ref_Vel_Act > 0) K_ff = 0.75f; // close - PWM_RAW_POS_FF = K_ff*Ref_Vel_Act/0.50f; + VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) / (float) TORQUE_SENSOR_PULSE_PER_TORQUE * 0.01f + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err /(float) ENC_PULSE_PER_POSITION * 0.01f + DDV_JOINT_POS_FF(vel.ref)); - // torque feedback - // torq.err = torq.ref - torq.sen; - // torq.err_diff = torq.err - torq.err_old; - // torq.err_old = torq.err; - // torq.err_sum += torq.err; - // if (torq.err_sum > 1000) torq.err_sum = 1000; - // if (torq.err_sum<-1000) torq.err_sum = -1000; - // VALVE_PWM_RAW_TORQ = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff; - // VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f; - - PWM_RAW_FORCE_FB = 0.0f; - - V_out = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB; + if (VALVE_POS_RAW_FORCE_FB >= 0) { + valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS; + } else{ + valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS; + } + VALVE_POS_CONTROL(valve_pos.ref); break; } @@ -549,10 +556,10 @@ valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB; - if (valve_pos.ref > DDV_CENTER) { - valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS - DDV_CENTER; - } else if(valve_pos.ref < DDV_CENTER) { - valve_pos.ref = valve_pos.ref - DDV_CENTER + VALVE_DEADZONE_MINUS; + if (valve_pos.ref >= 0) { + valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS; + } else if(valve_pos.ref < 0) { + valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_MINUS; } VALVE_POS_CONTROL(valve_pos.ref); @@ -651,30 +658,6 @@ } case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING: { - - float VALVE_POS_RAW_FORCE_FB = 0.0f; - - pos.err = pos.ref - pos.sen; //[pulse] - vel.err = vel.ref - vel.sen; //[pulse/s] - double K_spring = 1.0f; - double D_damper = 0.0001f; - torq.ref = (K_spring * pos.err + D_damper * vel.err) * ENC_PULSE_PER_POSITION; //[Nm] - - // torque feedback - torq.err = torq.ref - torq.sen; //[pulse] - torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse] - if (torq.err_sum > 100) torq.err_sum = 100; - if (torq.err_sum<-100) torq.err_sum = -100; - VALVE_POS_RAW_FORCE_FB = ((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) * (float) TORQUE_SENSOR_PULSE_PER_TORQUE; - - valve_pos.ref = DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB; - - if (valve_pos.ref > DDV_CENTER) { - valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS - DDV_CENTER; - } else if(valve_pos.ref < DDV_CENTER) { - valve_pos.ref = valve_pos.ref - DDV_CENTER + VALVE_DEADZONE_MINUS; - } - VALVE_POS_CONTROL(valve_pos.ref); break; }