![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
rainbow
Diff: main.cpp
- Revision:
- 59:f308b1656d9c
- Parent:
- 58:2eade98630e2
- Child:
- 60:64181f1d3e60
diff -r 2eade98630e2 -r f308b1656d9c main.cpp --- a/main.cpp Mon Mar 09 12:43:04 2020 +0000 +++ b/main.cpp Mon Mar 16 04:50:54 2020 +0000 @@ -292,7 +292,7 @@ break; } } - V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; + Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; } #define LT_MAX_IDX 57 @@ -489,10 +489,10 @@ CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0f; CUR_TORQUE_sum = 0; - TORQUE_VREF += 0.0003 * (0.0f - CUR_TORQUE_mean); + TORQUE_VREF += 0.0003f * (0.0f - CUR_TORQUE_mean); if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f; - if (TORQUE_VREF < 0) TORQUE_VREF = 0; + if (TORQUE_VREF < 0.0f) TORQUE_VREF = 0.0f; //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0)); dac_1 = TORQUE_VREF / 3.3f; @@ -594,8 +594,8 @@ cnt_findhome = 0; cnt_vel_findhome = 0; //REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO - pos.ref_home_pos = pos.sen; - vel.ref_home_pos = 0.0f; + pos.ref = pos.sen; + vel.ref = 0.0f; FINDHOME_STAGE = FINDHOME_GOTOLIMIT; } else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) { int cnt_check_enc = (TMR_FREQ_5k/500); @@ -614,20 +614,19 @@ if ((cnt_vel_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec //REFERENCE_MODE = MODE_REF_NO_ACT; - if (HOMEPOS_OFFSET > 0) pos.ref_home_pos = pos.ref_home_pos + 2.0f; - else pos.ref_home_pos = pos.ref_home_pos - 2.0f; - pos.err = pos.ref_home_pos - pos.sen; - float VALVE_POS_RAW_POS_FB = 0.0f; - VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f; - valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER; - VALVE_POS_CONTROL(valve_pos.ref); + if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 2.0f; + else pos.ref = pos.ref - 2.0f; + +// pos.err = pos.ref_home_pos - pos.sen; +// float VALVE_POS_RAW_POS_FB = 0.0f; +// VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f; +// valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER; +// VALVE_POS_CONTROL(valve_pos.ref); + + CONTROL_MODE = MODE_JOINT_CONTROL; + alpha_trans = 0.0f; + - //float wn_Pos = 2.0f*PI*5.0f; // f_cut : 10Hz Position Control - //I_REF = 0.04f*wn_Pos*((float)joint_pos_err/ENC_PULSE_PER_POSITION); - //// L velocity >> mA convert - //if(I_REF>5.0f) I_REF = 5.0f; - //if(I_REF<-5.0f) I_REF = -5.0f; - //FLAG_CURRNET_CONTROL = true; } else { ENC_SET(HOMEPOS_OFFSET); INIT_REF_POS = HOMEPOS_OFFSET; @@ -642,15 +641,18 @@ } } else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) { 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; + pos.ref = (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 = 0.0f; // input for position control - 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/(float) ENC_PULSE_PER_POSITION; - valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER; - VALVE_POS_CONTROL(valve_pos.ref); + + CONTROL_MODE = MODE_JOINT_CONTROL; + alpha_trans = 0.0f; +// pos.err = pos.ref - (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/(float) ENC_PULSE_PER_POSITION; +// valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER; +// VALVE_POS_CONTROL(valve_pos.ref); cnt_findhome++; if (cnt_findhome >= T_move) { @@ -829,12 +831,13 @@ // } case MODE_DDV_POS_VS_PWM_ID: { + CONTROL_MODE = MODE_VALVE_OPEN_LOOP; VALVE_ID_timer = VALVE_ID_timer + 1; if(VALVE_ID_timer < TMR_FREQ_5k*1) { - V_out = 3000.0f * sin(2*3.14f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f); + Vout.ref = 3000.0f * sin(2*3.14f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f); } else if(VALVE_ID_timer < TMR_FREQ_5k*2) { - V_out = 1000.0f*(ID_index_array[ID_index]); + Vout.ref = 1000.0f*(ID_index_array[ID_index]); } else if(VALVE_ID_timer == TMR_FREQ_5k*2) { VALVE_POS_TMP = 0; data_num = 0; @@ -842,6 +845,8 @@ data_num = data_num + 1; VALVE_POS_TMP = VALVE_POS_TMP + value; } else if(VALVE_ID_timer == TMR_FREQ_5k*3) { + Vout.ref = 0.0f; + } else { VALVE_POS_AVG[ID_index] = VALVE_POS_TMP / data_num; VALVE_ID_timer = 0; ID_index= ID_index +1; @@ -860,7 +865,7 @@ VALVE_POS_AVG_OLD = VALVE_MIN_POS; } } - //ROM_RESET_DATA(); + ROM_RESET_DATA(); ID_index = 0; CONTROL_UTILITY_MODE = MODE_NO_ACT; } @@ -870,27 +875,27 @@ } case MODE_DDV_DEADZONE_AND_CENTER: { - + CONTROL_MODE = MODE_VALVE_OPEN_LOOP; VALVE_DZ_timer = VALVE_DZ_timer + 1; if(first_check == 0) { if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { - V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; + Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f; } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { - V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; + Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f; pos_plus_end = pos.sen; } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { - V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; + Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f; } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { - V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; + Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f; pos_minus_end = pos.sen; } else if(VALVE_DZ_timer < (int) (3.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; + 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; } else if(VALVE_DZ_timer < (int) (4.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; + 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; data_num = data_num + 1; VALVE_POS_TMP = VALVE_POS_TMP + value; } else if(VALVE_DZ_timer == (int) (4.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; + 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; DDV_POS_AVG = VALVE_POS_TMP / data_num; START_POS = pos.sen; VALVE_POS_TMP = 0; @@ -919,8 +924,8 @@ } 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; @@ -934,7 +939,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)) { - 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; + //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)) { @@ -975,7 +982,9 @@ } } 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; + //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; } 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)) { @@ -991,6 +1000,8 @@ 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; @@ -1022,7 +1033,9 @@ } } 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; + //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; } 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)) { @@ -1062,7 +1075,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; + //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; } 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)) { @@ -1078,6 +1093,8 @@ 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) @@ -1098,6 +1115,8 @@ first_check = 0; VALVE_DEADZONE_MINUS = (float) FIRST_DZ; VALVE_DEADZONE_PLUS = (float) SECOND_DZ; + + ROM_RESET_DATA(); CONTROL_UTILITY_MODE = MODE_NO_ACT; DZ_index = 1; @@ -1109,20 +1128,21 @@ } case MODE_DDV_POS_VS_FLOWRATE: { + CONTROL_MODE = MODE_VALVE_OPEN_LOOP; VALVE_FR_timer = VALVE_FR_timer + 1; if(first_check == 0) { if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { - V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; + Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f; //CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6)); } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { - V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; + Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f; pos_plus_end = pos.sen; // CAN_TX_PRES((int16_t) (V_out), (int16_t) (7)); } else if(VALVE_FR_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { - V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; + Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f; } else if(VALVE_FR_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { // CAN_TX_PRES((int16_t) (V_out), (int16_t) (8)); - V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; + Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f; pos_minus_end = pos.sen; first_check = 1; VALVE_FR_timer = 0; @@ -1133,7 +1153,9 @@ } } else { if(VALVE_FR_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; + //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; } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { data_num = 0; valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER; @@ -1151,6 +1173,7 @@ } else if(VALVE_FR_timer == (int) (5.0f * (float) TMR_FREQ_5k)) { FINAL_POS = pos.sen; one_period_end = 1; + V_out = 0.0f; } if(one_period_end == 1) { @@ -1161,7 +1184,6 @@ } JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k; // pulse/sec - //ROM_RESET_DATA(); VALVE_FR_timer = 0; one_period_end = 0; ID_index= ID_index +1; @@ -1171,7 +1193,7 @@ if(max_check == 1 && min_check == 1) { VALVE_POS_NUM = ID_index; - //ROM_RESET_DATA(); + ROM_RESET_DATA(); ID_index = 0; first_check = 0; VALVE_FR_timer = 0; @@ -1215,6 +1237,7 @@ V_out = valve_pos.ref; } else { //SW Valve VALVE_POS_CONTROL(valve_pos.ref); + V_out = Vout.ref; } break; @@ -1435,7 +1458,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) (pres_A.sen*10.0f)); + CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (V_out)); } 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)); }