Sungwoo Kim
/
HydraulicControlBoard_Base
distribution-201229
Diff: main.cpp
- Revision:
- 32:4b8c0fedaf2c
- Parent:
- 31:66738bfecec5
- Child:
- 33:91b17819ec30
diff -r 66738bfecec5 -r 4b8c0fedaf2c main.cpp --- a/main.cpp Tue Oct 08 07:15:05 2019 +0000 +++ b/main.cpp Mon Nov 11 09:48:00 2019 +0000 @@ -512,7 +512,7 @@ pos.err_sum += pos.err; if (pos.err_sum > 1000) pos.err_sum = 1000; if (pos.err_sum<-1000) pos.err_sum = -1000; - VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff; + VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err + (float) I_GAIN_JOINT_POSITION * 0.01f * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff; VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01f; @@ -535,8 +535,17 @@ // VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f; VALVE_POS_RAW_FORCE_FB = 0.0f; - valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB; + valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB + VALVE_CENTER_OFFSET; // valve center offset = - 2.0f; //valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER; + + 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); @@ -1760,3 +1769,4 @@ +