![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
For LIGHT
Diff: main.cpp
- Revision:
- 49:e7bcfc244d40
- Parent:
- 48:889798ff9329
- Child:
- 50:3c630b5eba9f
diff -r 889798ff9329 -r e7bcfc244d40 main.cpp --- a/main.cpp Wed Jan 29 13:50:24 2020 +0000 +++ b/main.cpp Wed Feb 05 08:35:58 2020 +0000 @@ -75,6 +75,7 @@ extern int CID_RX_CMD; extern int CID_RX_REF_POSITION; +extern int CID_RX_REF_VALVE_POS; extern int CID_RX_REF_PWM; extern int CID_TX_INFO; @@ -265,7 +266,6 @@ } Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER); -// Ref_Valve_Pos_FF = 0.0; return Ref_Valve_Pos_FF; } @@ -330,8 +330,10 @@ break; } } - - V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; + if(vel.sen>=0.0f) + V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; + else + V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; // - (float) FLOWFORCE_GAIN * 0.01f * vel.sen / 30.0f; //V_out = VALVE_PWM_RAW_FB; } @@ -928,10 +930,10 @@ } if (need_enc_init) { if (TMR3_COUNT_DEADZONE < (int) (0.5f * (float) TMR_FREQ_5k)) { - V_out = VALVE_VOLTAGE_LIMIT; + V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; pos_plus_end = pos.sen; } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) { - V_out = -VALVE_VOLTAGE_LIMIT; + V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; pos_minus_end = pos.sen; } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false; temp_time = TMR_FREQ_5k; @@ -1104,16 +1106,16 @@ if (TMR3_COUNT_FLOWRATE == 0) { if (pos_plus_end == pos_minus_end) need_enc_init = true; else { - V_out = -VALVE_VOLTAGE_LIMIT; + V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; temp_time = (int) (0.5f * (float) TMR_FREQ_5k); } } if (need_enc_init) { if (TMR3_COUNT_FLOWRATE < (int) (0.5f * (float) TMR_FREQ_5k)) { - V_out = VALVE_VOLTAGE_LIMIT; + V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; pos_plus_end = pos.sen; } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) { - V_out = -VALVE_VOLTAGE_LIMIT; + V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; pos_minus_end = pos.sen; } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) { need_enc_init = false; @@ -1216,12 +1218,12 @@ case MODE_PRESSURE_SENSOR_CALIB: { if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) { - V_out = -VALVE_VOLTAGE_LIMIT; + V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; if (TMR3_COUNT_PRES_CALIB >= TMR_FREQ_5k) { CUR_PRES_A_sum += CUR_PRES_A; } } else if (TMR3_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) { - V_out = VALVE_VOLTAGE_LIMIT; + V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) { CUR_PRES_B_sum += CUR_PRES_B; } @@ -1250,8 +1252,8 @@ case MODE_ROTARY_FRICTION_TUNING: { if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4.0f + 3.0f * sin(2 * 3.14159f * 0.5f * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f * 0.05f); V_out = PWM_out * sin(2 * 3.14159f * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f); - if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT; - else V_out = -VALVE_VOLTAGE_LIMIT; + if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; + else V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; TMR3_COUNT_ROTARY_FRIC_TUNE++; if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) { TMR3_COUNT_ROTARY_FRIC_TUNE = 0; @@ -1307,23 +1309,23 @@ 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; + V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { - V_out = VALVE_VOLTAGE_LIMIT; + V_out = 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; + V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { - V_out = -VALVE_VOLTAGE_LIMIT; + V_out = -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.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen); + 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; } else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) { - V_out = (float) P_GAIN_JOINT_POSITION * 0.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen); + 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; 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.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen); + 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; DDV_POS_AVG = VALVE_POS_TMP / data_num; START_POS = pos.sen; VALVE_POS_TMP = 0; @@ -1353,6 +1355,8 @@ DZ_case = 0; } + CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6)); + VEL_POINT = 0; first_check = 1; DZ_DIRECTION = 1; @@ -1365,7 +1369,7 @@ } 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.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen); + 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; } 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)) { @@ -1406,7 +1410,7 @@ } } 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.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen); + 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; } 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)) { @@ -1453,7 +1457,7 @@ } } 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.01f* (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen); + 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; } 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)) { @@ -1493,7 +1497,7 @@ } } else { if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { - V_out = (float) P_GAIN_JOINT_POSITION * 0.01f* (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen); + 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; } 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)) { @@ -1543,17 +1547,17 @@ 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; + V_out = 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; + V_out = 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; + V_out = -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; + V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; pos_minus_end = pos.sen; first_check = 1; VALVE_FR_timer = 0; @@ -1564,7 +1568,7 @@ } } else { if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { - V_out = (float) P_GAIN_JOINT_POSITION * 0.01f* (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen); + 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; } 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]) + DDV_CENTER; @@ -1607,6 +1611,7 @@ first_check = 0; VALVE_FR_timer = 0; CONTROL_MODE = MODE_NO_ACT; +// CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6)); } } break; @@ -1645,12 +1650,12 @@ /******************************************************* *** PWM ********************************************************/ - if (V_out >= 12000.0f) { - V_out = 12000.0f; - } else if(V_out<=-12000.0f) { - V_out = -12000.0f; + if (V_out >= VALVE_VOLTAGE_LIMIT*1000.0f) { + V_out = VALVE_VOLTAGE_LIMIT*1000.0f; + } else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) { + V_out = -VALVE_VOLTAGE_LIMIT*1000.0f; } - PWM_out= V_out/SUPPLY_VOLTAGE; // Full duty : 12000.0mV + PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f); // Full duty : 12000.0mV // Saturation of output voltage to 5.0V // if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667 @@ -1690,7 +1695,7 @@ if (flag_data_request[0] == HIGH) { //position+velocity //CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen); - CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f)); + CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (V_out/100.0f), (int16_t) (torq.sen*10.0f)); } if (flag_data_request[1] == HIGH) { @@ -1708,7 +1713,7 @@ if (flag_data_request[2] == HIGH) { //pressure A and B - CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF * 1000.0)); // CUR_PRES_X : 0(0bar)~4096(210bar) + CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (PRES_A_VREF * 1000.0f)); // CUR_PRES_X : 0(0bar)~4096(210bar) //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar) // CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.)); // CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.);