![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
Diff: main.cpp
- Revision:
- 44:fe7d5cfd2eea
- Parent:
- 43:b084e5f5d0d5
- Child:
- 45:35fa6884d0c6
--- a/main.cpp Tue Jan 07 08:39:22 2020 +0000 +++ b/main.cpp Tue Jan 14 09:11:20 2020 +0000 @@ -627,6 +627,7 @@ } case MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION: { + pos.err = pos.ref - (float) pos.sen; pos.err_diff = pos.err - pos.err_old; pos.err_old = pos.err; @@ -650,6 +651,30 @@ } 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; } @@ -1810,3 +1835,4 @@ +