![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
asdgas
Dependencies: mbed Eigen FastPWM
Diff: main.cpp
- Revision:
- 64:368bcd2a230a
- Parent:
- 63:376a20d2fbfd
- Child:
- 65:549e0426cd91
diff -r 376a20d2fbfd -r 368bcd2a230a main.cpp --- a/main.cpp Thu May 07 03:52:28 2020 +0000 +++ b/main.cpp Tue May 12 06:24:31 2020 +0000 @@ -654,7 +654,7 @@ FINDHOME_VELOCITY = 0; cnt_findhome = 0; cnt_vel_findhome = 0; -// FINDHOME_STAGE = FINDHOME_ZEROPOSE; + FINDHOME_STAGE = FINDHOME_ZEROPOSE; cnt_findhome = 0; @@ -662,8 +662,8 @@ 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; + //FINDHOME_STAGE = FINDHOME_INIT; + //CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL; } @@ -677,10 +677,6 @@ // CONTROL_MODE = MODE_JOINT_CONTROL; alpha_trans = 0.0f; - - - - double torq_ref = 0.0f; pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] @@ -708,12 +704,11 @@ else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f)))); I_REF = I_REF_POS; - - + + } else { float VALVE_POS_RAW_FORCE_FB = 0.0f; - VALVE_POS_RAW_FORCE_FB = DDV_JOINT_POS_FF(vel.sen) + (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref)); if (VALVE_POS_RAW_FORCE_FB >= 0) { @@ -1560,18 +1555,18 @@ if (flag_data_request[1] == HIGH) { //valve position double t_value = 0; -// if(valve_pos.ref>=(float) VALVE_CENTER) { -// t_value = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); + if(valve_pos.ref>=(float) VALVE_CENTER) { + t_value = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); + } else { + t_value = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); + } +// if(OPERATING_MODE==5) { +// t_value = (double) value; +// } else if(CURRENT_CONTROL_MODE==1) { +// t_value = cur.sen; // } else { -// t_value = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); +// t_value = V_out; // } - if(OPERATING_MODE==5) { - t_value = (double) value; - } else if(CURRENT_CONTROL_MODE==1) { - t_value = cur.sen; - } else { - t_value = V_out; - } CAN_TX_TORQUE((int16_t) (t_value)); //1300 //CAN_TX_TORQUE((int16_t) (cur.sen * 1000.0f)); //1300 //CAN_TX_TORQUE((int16_t) (I_REF * 1000.0f)); //1300