Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2_copy
2011
Diff: main.cpp
- Revision:
- 39:3f2c0619c8c4
- Parent:
- 38:118df027d851
- Child:
- 40:abbd4e2af68b
--- a/main.cpp Thu Nov 28 09:18:03 2019 +0000 +++ b/main.cpp Thu Dec 19 11:39:31 2019 +0000 @@ -512,8 +512,6 @@ } case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: { - - float VALVE_POS_RAW_POS_FB = 0.0f; // Valve Position by Position Feedback //float VALVE_POS_RAW_POS_FF = 0.0f; // Valve Position by Position Feedforward float VALVE_POS_RAW_FORCE_FB = 0.0f; // Valve Position by Force Feedback @@ -948,6 +946,7 @@ pos.ref_home_pos = pos.sen; vel.ref_home_pos = 0.0f; FINDHOME_STAGE = FINDHOME_GOTOLIMIT; + CAN_TX_PRES((int16_t)(CONTROL_MODE), (int16_t) (3)); } else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) { int cnt_check_enc = (TMR_FREQ_5k/500); if(cnt_findhome%cnt_check_enc == 0) { @@ -997,9 +996,10 @@ int T_move = 2*TMR_FREQ_5k; pos.ref_home_pos = (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_home_pos = 0.0f; + CAN_TX_PRES((int16_t)(pos.ref_home_pos), (int16_t) (6)); // input for position control - pos.err = (pos.ref_home_pos - (float)pos.sen) / ENC_PULSE_PER_POSITION; + pos.err = pos.ref_home_pos - (float)pos.sen; float VALVE_POS_RAW_POS_FB = 0.0f; VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err; VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01f;