Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2
rainbow
Revision 245:071785d74ad0, committed 2022-06-17
- Comitter:
- Lightvalve
- Date:
- Fri Jun 17 09:28:54 2022 +0000
- Parent:
- 244:e9c5ec04e378
- Child:
- 246:d5742551f432
- Commit message:
- 220617_2
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Jun 17 06:06:53 2022 +0000 +++ b/main.cpp Fri Jun 17 09:28:54 2022 +0000 @@ -270,18 +270,10 @@ dac_1 = FORCE_VREF / 3.3f; dac_2 = 0.0f; } else if (SENSING_MODE == 1) { -// if (DIR_VALVE_ENC > 0) { dac_1 = PRES_A_VREF / 3.3f; dac_2 = PRES_B_VREF / 3.3f; -// } else { -// dac_1 = PRES_B_VREF / 3.3f; -// dac_2 = PRES_A_VREF / 3.3f; -// } } - dac_1 = 1.0f/3.3f; - dac_2 = 1.0f/3.3f; - make_delay(); for (int i=0; i<50; i++) { @@ -471,8 +463,8 @@ { if (TIM3->SR & TIM_SR_UIF ) { - if (LED > 0) LED = 0; - else LED = 1; +// if (LED > 0) LED = 0; +// else LED = 1; float PSEN1 = 0.0f; float PSEN2 = 0.0f; @@ -496,10 +488,15 @@ /////////////////////////Force or Pressure////////////////////////////////////////////////////////////////// if (SENSING_MODE == 0) { // Force sensing - + HAL_ADC_Start(&hadc1); HAL_ADC_PollForConversion(&hadc1, 1); PSEN1 = (float) HAL_ADC_GetValue(&hadc1); + + HAL_ADC_Start(&hadc1); + HAL_ADC_PollForConversion(&hadc1, 1); + PSEN2 = (float) HAL_ADC_GetValue(&hadc1); + force.UpdateSen((((float)PSEN1) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR3, 100.0f); // unit : N } else if (SENSING_MODE == 1) { // Pressure sensing @@ -579,6 +576,7 @@ float FREQ_TMR2 = (float)FREQ_5k; float DT_TMR2 = (float)DT_5k; int cnt_trans = 0; +float nulling_test = 0.0f; extern "C" void TIM2_IRQHandler(void) { if (TIM2->SR & TIM_SR_UIF ) { @@ -666,9 +664,9 @@ } case MODE_TORQUE_SENSOR_NULLING: { - static float FORCE_pulse_sum = 0.0; - static float PresA_pulse_sum = 0.0; - static float PresB_pulse_sum = 0.0; + static float FORCE_pulse_sum = 0.0f; + static float PresA_pulse_sum = 0.0f; + static float PresB_pulse_sum = 0.0f; // DAC Voltage reference set float VREF_TuningGain = -0.000003f; @@ -679,8 +677,8 @@ if (TMR3_COUNT_TORQUE_NULL % 10 == 0) { float FORCE_pluse_mean = FORCE_pulse_sum / 10.0f; FORCE_pulse_sum = 0.0f; - FORCE_VREF += VREF_TuningGain * (0.0f - FORCE_pluse_mean); + nulling_test = FORCE_VREF; if (FORCE_VREF > 3.3f) FORCE_VREF = 3.3f; if (FORCE_VREF < 0.0f) FORCE_VREF = 0.0f; dac_1 = FORCE_VREF / 3.3f; @@ -1426,19 +1424,17 @@ //////////////////////////////////////////////////////////////////////////// ////////////////////// Data transmission through CAN ////////////////////// //////////////////////////////////////////////////////////////////////////// - + // if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) { if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/500) == 0) { // Position, Velocity, and Torque (ID:1200) - if (flag_data_request[0] == LOW) { - CAN_TX_POSITION_FT((int16_t) (cur.sen*1000.0f), (int16_t) (V_EXI), (int16_t) (valve_pos.sen)); -// if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator -// CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); -// -// } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator -// CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); -// } + if (flag_data_request[0] == HIGH) { + if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator + CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); + } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator + CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); + } } // Valve Position (ID:1300) @@ -1446,7 +1442,8 @@ 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)); +// CAN_TX_PWM((int16_t)(valve_pos.sen)); + CAN_TX_PWM((int16_t)(nulling_test)); } }