Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2_copy1
2011
Diff: main.cpp
- Revision:
- 248:544011ad9c85
- Parent:
- 247:bfdf0f479a38
- Child:
- 249:f69cd0553be0
diff -r bfdf0f479a38 -r 544011ad9c85 main.cpp --- a/main.cpp Fri Jun 24 00:18:20 2022 +0000 +++ b/main.cpp Fri Jun 24 05:55:29 2022 +0000 @@ -198,7 +198,6 @@ enc.format(8,0); enc.frequency(5000000); //10M enc_cs = 0; - make_delay(); // spi _ enc @@ -360,21 +359,19 @@ } // Valve feedforward for SW valve -void VALVE_POS_CONTROL_DZ(float REF_VALVE_POS_10000) +void VALVE_POS_CONTROL_DZ(float REF_VALVE_POS) { int i = 0; float valve_pos_ref_dz = 0.0f; - if(REF_VALVE_POS_10000 > 10000.0f) { - REF_VALVE_POS_10000 = 10000.0f; - } else if(REF_VALVE_POS_10000 < -10000.0f) { - REF_VALVE_POS_10000 = -10000.0f; + if(REF_VALVE_POS > VALVE_MAX_POS) { + REF_VALVE_POS = VALVE_MAX_POS; + } else if(REF_VALVE_POS < VALVE_MIN_POS) { + REF_VALVE_POS = VALVE_MIN_POS; } - if (REF_VALVE_POS_10000 >= 0) { -// valve_pos_ref_dz = REF_VALVE_POS_10000/10000.0f * (VALVE_MAX_POS-VALVE_DEADZONE_PLUS) + VALVE_DEADZONE_PLUS; - valve_pos_ref_dz = REF_VALVE_POS_10000; + if (REF_VALVE_POS >= 0) { + valve_pos_ref_dz = REF_VALVE_POS + VALVE_DEADZONE_PLUS - VALVE_ELECTRIC_CENTER; } else { -// valve_pos_ref_dz = -REF_VALVE_POS_10000/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS; - valve_pos_ref_dz = REF_VALVE_POS_10000; + valve_pos_ref_dz = REF_VALVE_POS + VALVE_DEADZONE_MINUS - VALVE_ELECTRIC_CENTER; } valve_pos_err = (float) (valve_pos_ref_dz - valve_pos.sen); valve_pos_err_diff = valve_pos_err - valve_pos_err_old; @@ -547,7 +544,7 @@ for (int ij = 0; ij<150; ij++) { ADC3->CR2 |= 0x40000000; - LVDT_new = ((float)ADC3->DR) - 2047.5f; + LVDT_new = ((float)ADC3->DR); LVDT_sum = LVDT_sum + LVDT_new; } @@ -836,8 +833,6 @@ ID_index = 0; CONTROL_UTILITY_MODE = MODE_NO_ACT; } - - break; } case MODE_DDV_DEADZONE_AND_CENTER: { @@ -1076,7 +1071,7 @@ valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f; } - VALVE_POS_CONTROL(valve_pos_raw.ref); + VALVE_POS_CONTROL_DZ(valve_pos_raw.ref); ref_array[cnt_step_test] = valve_pos_ref; if(valve_pos.sen >= (float) VALVE_ELECTRIC_CENTER) { @@ -1111,7 +1106,7 @@ valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f; } - VALVE_POS_CONTROL(valve_pos_raw.ref); + VALVE_POS_CONTROL_DZ(valve_pos_raw.ref); ref_array[cnt_freq_test] = valve_pos_ref; // if(value>=(float) VALVE_ELECTRIC_CENTER) { @@ -1436,7 +1431,13 @@ if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse)); } else { - CAN_TX_PWM((int16_t)(valve_pos.sen)); + float valve_pos_can = 0.0f; + if(valve_pos.sen >= VALVE_ELECTRIC_CENTER) { + valve_pos_can = 10000.0f*((float)valve_pos.sen-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER); + } else { + valve_pos_can = -10000.0f*((float)valve_pos.sen -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER); + } + CAN_TX_PWM((int16_t)(valve_pos_can)); } }