![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LIGHT2
Diff: main.cpp
- Revision:
- 60:64181f1d3e60
- Parent:
- 59:f308b1656d9c
- Child:
- 61:bc8c8270f0ab
diff -r f308b1656d9c -r 64181f1d3e60 main.cpp --- a/main.cpp Mon Mar 16 04:50:54 2020 +0000 +++ b/main.cpp Tue Mar 31 05:12:37 2020 +0000 @@ -835,7 +835,7 @@ VALVE_ID_timer = VALVE_ID_timer + 1; if(VALVE_ID_timer < TMR_FREQ_5k*1) { - Vout.ref = 3000.0f * sin(2*3.14f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f); + Vout.ref = 3000.0f * sin(2.0f*3.14f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f); } else if(VALVE_ID_timer < TMR_FREQ_5k*2) { Vout.ref = 1000.0f*(ID_index_array[ID_index]); } else if(VALVE_ID_timer == TMR_FREQ_5k*2) { @@ -908,26 +908,22 @@ } else if(VALVE_DZ_timer < (int) (6.0f * (float) TMR_FREQ_5k)) { valve_pos.ref = DDV_POS_AVG; VALVE_POS_CONTROL(valve_pos.ref); - if(CUR_VELOCITY >= 0) - VEL_POINT = VEL_POINT + 1; - else - VEL_POINT = VEL_POINT - 1; + } else if(VALVE_DZ_timer == (int) (6.0f * (float) TMR_FREQ_5k)) { valve_pos.ref = DDV_POS_AVG; VALVE_POS_CONTROL(valve_pos.ref); FINAL_POS = pos.sen; - if((FINAL_POS - START_POS)>100) { + if((FINAL_POS - START_POS)>200) { DZ_case = 1; - } else if((FINAL_POS - START_POS)<-100) { + } else if((FINAL_POS - START_POS)<-200) { DZ_case = -1; } else { DZ_case = 0; } - //CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6)); + CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6)); - VEL_POINT = 0; first_check = 1; DZ_DIRECTION = 1; VALVE_DZ_timer = 0; @@ -939,9 +935,9 @@ } else { if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) { if(VALVE_DZ_timer < (int) (1.0 * (float) TMR_FREQ_5k)) { - //Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; - pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end; - CONTROL_MODE = MODE_JOINT_CONTROL; + Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; + //pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end; + //CONTROL_MODE = MODE_JOINT_CONTROL; } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { START_POS = pos.sen; } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { @@ -952,10 +948,7 @@ valve_pos.ref = VALVE_MAX_POS; } VALVE_POS_CONTROL(valve_pos.ref); - if(CUR_VELOCITY >= 0) - VEL_POINT = VEL_POINT + 1; - else - VEL_POINT = VEL_POINT - 1; + } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { Ref_Valve_Pos_Old = valve_pos.ref; FINAL_POS = pos.sen; @@ -968,8 +961,6 @@ DZ_DIRECTION = 1 * DZ_case; } - VEL_POINT = 0; - VALVE_DZ_timer = 0; DZ_index= DZ_index *2; if(DZ_index >= 128) { @@ -982,23 +973,20 @@ } } else if((DZ_case == -1 && DZ_NUM == 2) | (DZ_case == 1 && DZ_NUM == 2)) { if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { - //V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; - pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end; - CONTROL_MODE = MODE_JOINT_CONTROL; + Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; + //pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end; + //CONTROL_MODE = MODE_JOINT_CONTROL; } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { START_POS = pos.sen; } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { - valve_pos.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index; + valve_pos.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index; if(valve_pos.ref <= VALVE_MIN_POS) { valve_pos.ref = VALVE_MIN_POS; } else if(valve_pos.ref >= VALVE_MAX_POS) { valve_pos.ref = VALVE_MAX_POS; } VALVE_POS_CONTROL(valve_pos.ref); - if(CUR_VELOCITY >= 0) - VEL_POINT = VEL_POINT + 1; - else - VEL_POINT = VEL_POINT - 1; + } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { Vout.ref = 0.0f; } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) { @@ -1006,18 +994,15 @@ FINAL_POS = pos.sen; if((FINAL_POS - START_POS)>100) { - DZ_DIRECTION = -1; + DZ_DIRECTION = 1 * DZ_case; } else if((FINAL_POS - START_POS)<-100) { - DZ_DIRECTION = 1; + DZ_DIRECTION = -1 * DZ_case; } else { - DZ_DIRECTION = 1; + DZ_DIRECTION = -1 * DZ_case; } - VEL_POINT = 0; - - VALVE_DZ_timer = 0; - DZ_index= DZ_index *2; + DZ_index= DZ_index * 2; if(DZ_index >= 128) { SECOND_DZ = valve_pos.ref; VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ)); @@ -1027,15 +1012,15 @@ ROM_RESET_DATA(); - CONTROL_MODE = MODE_NO_ACT; + CONTROL_UTILITY_MODE = MODE_NO_ACT; DZ_index = 1; } } } else if(DZ_case == 0 && DZ_NUM ==1) { if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { - //V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; - pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end; - CONTROL_MODE = MODE_JOINT_CONTROL; + Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; + //pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end; + //CONTROL_MODE = MODE_JOINT_CONTROL; } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { START_POS = pos.sen; } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { @@ -1046,10 +1031,7 @@ valve_pos.ref = VALVE_MAX_POS; } VALVE_POS_CONTROL(valve_pos.ref); - if(CUR_VELOCITY >= 0) - VEL_POINT = VEL_POINT + 1; - else - VEL_POINT = VEL_POINT - 1; + } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { Ref_Valve_Pos_Old = valve_pos.ref; FINAL_POS = pos.sen; @@ -1061,8 +1043,6 @@ } else { DZ_DIRECTION = 1; } - - VEL_POINT = 0; VALVE_DZ_timer = 0; DZ_index= DZ_index *2; if(DZ_index >= 128) { @@ -1075,9 +1055,9 @@ } } else { if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { - //V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; - pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end; - CONTROL_MODE = MODE_JOINT_CONTROL; + Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; + //pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end; + //CONTROL_MODE = MODE_JOINT_CONTROL; } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { START_POS = pos.sen; } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { @@ -1088,24 +1068,20 @@ valve_pos.ref = VALVE_MAX_POS - 1; } VALVE_POS_CONTROL(valve_pos.ref); - if(CUR_VELOCITY >= 0) - VEL_POINT = VEL_POINT + 1; - else - VEL_POINT = VEL_POINT - 1; + } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { Vout.ref = 0.0f; } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) { Ref_Valve_Pos_Old = valve_pos.ref; FINAL_POS = pos.sen; - if(VEL_POINT >= 0) - DZ_DIRECTION = -1; - else + + if((FINAL_POS - START_POS)>100) { DZ_DIRECTION = 1; - if(abs(FINAL_POS - START_POS) < 100) + } else if((FINAL_POS - START_POS)<-100) { + DZ_DIRECTION = -1; + } else { DZ_DIRECTION = 1; - - VEL_POINT = 0; - + } VALVE_DZ_timer = 0; DZ_index= DZ_index *2; @@ -1320,7 +1296,11 @@ torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k; } } + VALVE_POS_CONTROL(valve_pos.ref); + + Vout.ref = (float) P_GAIN_JOINT_POSITION * 0.01f * ((float) pos.err); + V_out = (float) Vout.ref; } @@ -1458,7 +1438,7 @@ } } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator if (SENSING_MODE == 0) { - CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (V_out)); + CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (pres_A.sen)); } else if (SENSING_MODE == 1) { CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); } @@ -1479,7 +1459,7 @@ } else { t_value = V_out; } - CAN_TX_TORQUE((int16_t) (t_value)); //1300 + CAN_TX_TORQUE((int16_t) (t_value), (int16_t) (V_out)); //1300 }