Sungwoo Kim
/
HydraulicControlBoard_PostLIGHT_210420
LIGHT2
Diff: main.cpp
- Revision:
- 18:b8adf1582ea3
- Parent:
- 17:1865016ca2e7
- Child:
- 19:23b7c1ad8683
diff -r 1865016ca2e7 -r b8adf1582ea3 main.cpp --- a/main.cpp Tue Sep 03 11:59:55 2019 +0000 +++ b/main.cpp Wed Sep 04 11:02:57 2019 +0000 @@ -70,7 +70,6 @@ int timer_while = 0; int while_index = 0; - // ============================================================================= // ============================================================================= // ============================================================================= @@ -252,12 +251,12 @@ REF_VALVE_POS = VALVE_MIN_POS; } - for(i=0; i<16; i++) { + for(i=0; i<24; i++) { if(REF_VALVE_POS >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && REF_VALVE_POS <= max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1])) { if(i==0) { - VALVE_PWM_RAW_FF = (double) 1000.0/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * (REF_VALVE_POS - VALVE_POS_VS_PWM[i]); + VALVE_PWM_RAW_FF = (double) 1000.0 / (double) (VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[i]); } else { - VALVE_PWM_RAW_FF = (double) 1000.0*(ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * (REF_VALVE_POS - VALVE_POS_VS_PWM[i-1]) + 1000*ID_index_array[i-1]; + VALVE_PWM_RAW_FF = (double) 1000.0* (double) (ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[i-1]) + 1000.0* (double) ID_index_array[i-1]; } break; } @@ -363,7 +362,7 @@ } TIM4->SR = 0x0; // reset the status register } - +int j =0; unsigned long CNT_TMR3 = 0; double FREQ_TMR3 = (double)FREQ_5k; double DT_TMR3 = (double)DT_5k; @@ -511,7 +510,7 @@ } case MODE_VALVE_OPEN_LOOP: { - V_out = Vout.ref; + V_out = (double) Vout.ref; break; } @@ -664,7 +663,7 @@ if (CUR_FLOWRATE >= 0) VALVE_FF_VOLTAGE = (double) VELOCITY_COMP_GAIN * 0.001 * VALVE_FF_VOLTAGE * sqrt((double) PRES_SUPPLY - CUR_PRES_A_BAR) * 0.0707; // 0.0707 = 1/sqrt(200.)) else if (CUR_FLOWRATE < 0) VALVE_FF_VOLTAGE = (double) VELOCITY_COMP_GAIN * 0.001 * VALVE_FF_VOLTAGE * sqrt((double) PRES_SUPPLY - CUR_PRES_B_BAR) * 0.0707; - V_out = V_out + VALVE_FF_VOLTAGE * PWM_RESOL / SUPPLY_VOLTAGE; + V_out = V_out + VALVE_FF_VOLTAGE; break; } @@ -912,21 +911,21 @@ } if (need_enc_init) { if (TMR3_COUNT_DEADZONE < (int) (0.5 * (double) TMR_FREQ_5k)) { - VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = VALVE_VOLTAGE_LIMIT; pos_plus_end = pos.sen; } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) { - VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = -VALVE_VOLTAGE_LIMIT; pos_minus_end = pos.sen; } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false; temp_time = TMR_FREQ_5k; } if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) { - VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen); + V_out = (double) P_GAIN_JOINT_POSITION * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen); VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0; } else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9 * (double) TMR_FREQ_5k))) { - VALVE_PWM_RAW = 0; + V_out = 0; CUR_VELOCITY_sum += CUR_VELOCITY; } else if (TMR3_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) { if (CUR_VELOCITY_sum == 0) DZ_dir = 1; @@ -949,13 +948,13 @@ if (TMR3_COUNT_DEADZONE % 20 != 0) { CUR_VELOCITY_sum += CUR_VELOCITY; } else { - VALVE_PWM_RAW -= DZ_dir; + V_out -= DZ_dir; if (CUR_VELOCITY_sum * DZ_dir < 0) DZ_temp_cnt++; CUR_VELOCITY_sum = 0; } if (DZ_temp_cnt == 5) { - if (DZ_dir >= 0) VALVE_DEADZONE_MINUS = (int16_t) VALVE_PWM_RAW; - else VALVE_DEADZONE_PLUS = (int16_t) VALVE_PWM_RAW; + if (DZ_dir >= 0) VALVE_DEADZONE_MINUS = (int16_t) V_out; + else VALVE_DEADZONE_PLUS = (int16_t) V_out; DZ_dir = -DZ_dir; DZ_temp_cnt = 0; DZ_temp_cnt2++; @@ -967,7 +966,7 @@ VALVE_DEADZONE_PLUS = VALVE_CENTER; VALVE_DEADZONE_MINUS = VALVE_CENTER; } - VALVE_PWM_RAW = 0; + V_out = 0; ROM_RESET_DATA(); @@ -1015,8 +1014,8 @@ else pos.ref = pos.ref - 1.0; pos.err = pos.ref - (double) pos.sen; V_out = ((double) P_GAIN_JOINT_POSITION * pos.err) * 0.01; - // if (HOMEPOS_OFFSET > 0) VALVE_PWM_RAW = 100; - // else VALVE_PWM_RAW = -100; + // if (HOMEPOS_OFFSET > 0) V_out = 100; + // else V_out = -100; } else { ENC_SET(HOMEPOS_OFFSET); FLAG_REFERENCE_JOINT_POSITION = 1; @@ -1093,16 +1092,16 @@ if (TMR3_COUNT_FLOWRATE == 0) { if (pos_plus_end == pos_minus_end) need_enc_init = true; else { - VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = -VALVE_VOLTAGE_LIMIT; temp_time = (int) (0.5 * (double) TMR_FREQ_5k); } } if (need_enc_init) { if (TMR3_COUNT_FLOWRATE < (int) (0.5 * (double) TMR_FREQ_5k)) { - VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = VALVE_VOLTAGE_LIMIT; pos_plus_end = pos.sen; } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) { - VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = -VALVE_VOLTAGE_LIMIT; pos_minus_end = pos.sen; } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) { need_enc_init = false; @@ -1115,8 +1114,8 @@ TMR3_COUNT_FLOWRATE++; if (TMR3_COUNT_FLOWRATE > temp_time) { if (flag_flowrate % 2 == 0) { // (+) - VALVE_VOLTAGE = 1. * (double) (flag_flowrate / 2 + 1); - VALVE_PWM_RAW = VALVE_VOLTAGE / SUPPLY_VOLTAGE*PWM_RESOL; + VALVE_VOLTAGE = 1000.0 * (double) (flag_flowrate / 2 + 1); + V_out = VALVE_VOLTAGE; if (pos.sen > (pos_minus_end + check_vel_pos_init) && pos.sen < (pos_minus_end + check_vel_pos_fin)) { fl_temp_cnt++; } else if (pos.sen >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) { @@ -1126,7 +1125,7 @@ } } else if (flag_flowrate % 2 == 1) { // (-) VALVE_VOLTAGE = -1. * (double) (flag_flowrate / 2 + 1); - VALVE_PWM_RAW = VALVE_VOLTAGE / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = VALVE_VOLTAGE; if (pos.sen < (pos_plus_end - check_vel_pos_init) && pos.sen > (pos_plus_end - check_vel_pos_fin)) { fl_temp_cnt++; } else if (pos.sen <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) { @@ -1146,7 +1145,7 @@ flag_flowrate++; } if (flag_flowrate == 10) { - VALVE_PWM_RAW = 0; + V_out = 0; flag_flowrate = 0; TMR3_COUNT_FLOWRATE = 0; valve_gain_repeat_cnt++; @@ -1205,23 +1204,23 @@ case MODE_PRESSURE_SENSOR_CALIB: { if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) { - VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL / 5.; + V_out = -VALVE_VOLTAGE_LIMIT; 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) { - VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL / 5.; + V_out = VALVE_VOLTAGE_LIMIT; if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) { CUR_PRES_B_sum += CUR_PRES_B; } } else { CONTROL_MODE = MODE_NO_ACT; TMR3_COUNT_PRES_CALIB = 0; - VALVE_PWM_RAW = 0; - PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((double) TMR_FREQ_5k - 1.) - PRES_A_NULL; - PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((double) PRES_SUPPLY - 1.); - PRES_SENSOR_B_PULSE_PER_BAR = CUR_PRES_B_sum / ((double) TMR_FREQ_5k - 1.) - PRES_B_NULL; - PRES_SENSOR_B_PULSE_PER_BAR = PRES_SENSOR_B_PULSE_PER_BAR / ((double) PRES_SUPPLY - 1.); + V_out = 0; + PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((double) TMR_FREQ_5k - 1.0) - PRES_A_NULL; + PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((double) PRES_SUPPLY - 1.0); + PRES_SENSOR_B_PULSE_PER_BAR = CUR_PRES_B_sum / ((double) TMR_FREQ_5k - 1.0) - PRES_B_NULL; + PRES_SENSOR_B_PULSE_PER_BAR = PRES_SENSOR_B_PULSE_PER_BAR / ((double) PRES_SUPPLY - 1.0); CUR_PRES_A_sum = 0; CUR_PRES_B_sum = 0; CUR_PRES_A_mean = 0; @@ -1238,9 +1237,9 @@ case MODE_ROTARY_FRICTION_TUNING: { if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4 + 3. * sin(2 * 3.14159 * 0.5 * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001 * 0.05); - V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL * sin(2 * 3.14159 * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001); - if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL; - else V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = PWM_out * sin(2 * 3.14159 * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001); + if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT; + else V_out = -VALVE_VOLTAGE_LIMIT; TMR3_COUNT_ROTARY_FRIC_TUNE++; if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) { TMR3_COUNT_ROTARY_FRIC_TUNE = 0; @@ -1254,9 +1253,9 @@ VALVE_ID_timer = VALVE_ID_timer + 1; if(VALVE_ID_timer < TMR_FREQ_5k*1) { - V_out = 3000 * sin(2*3.14*VALVE_ID_timer/TMR_FREQ_5k * 100); + V_out = 3000.0 * sin(2*3.14*VALVE_ID_timer/TMR_FREQ_5k * 100.0); } else if(VALVE_ID_timer < TMR_FREQ_5k*2) { - V_out = 1000*(ID_index_array[ID_index]); + V_out = 1000.0*(ID_index_array[ID_index]); } else if(VALVE_ID_timer == TMR_FREQ_5k*2) { VALVE_POS_TMP = 0; data_num = 0; @@ -1305,14 +1304,14 @@ VALVE_DZ_timer = VALVE_DZ_timer + 1; if(first_check == 0) { if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) { - V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = VALVE_VOLTAGE_LIMIT; } else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) { - V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = VALVE_VOLTAGE_LIMIT; pos_plus_end = pos.sen; } else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) { - V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = -VALVE_VOLTAGE_LIMIT; } else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) { - V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = -VALVE_VOLTAGE_LIMIT; pos_minus_end = pos.sen; } else if(VALVE_DZ_timer < (int) (3.0 * (double) TMR_FREQ_5k)) { V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen); @@ -1372,7 +1371,7 @@ Ref_Valve_Pos_Old = DDV_POS_AVG; DZ_NUM = 1; DZ_index = 1; - CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (DZ_NUM)); + //CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (DZ_NUM)); } }else { @@ -1431,7 +1430,7 @@ } else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) { START_POS = pos.sen; } else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) { - valve_pos.ref = Ref_Valve_Pos_Old + DZ_DIRECTION + 64 / DZ_index; + valve_pos.ref = Ref_Valve_Pos_Old + 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) { @@ -1474,7 +1473,7 @@ //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ); //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ); - //spi_eeprom_write(RID_DDV_CNETER, DDV_CENTER); + //spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER); //CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ); CONTROL_MODE = MODE_NO_ACT; DZ_index = 1; @@ -1576,7 +1575,7 @@ ROM_RESET_DATA(); //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ); - //spi_eeprom_write(RID_DDV_CNETER, DDV_CENTER); + //spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER); CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ); CONTROL_MODE = MODE_NO_ACT; DZ_index = 1; @@ -1593,17 +1592,17 @@ VALVE_FR_timer = VALVE_FR_timer + 1; if(first_check == 0) { if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k)) { - V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = VALVE_VOLTAGE_LIMIT; CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6)); } else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k)) { - V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = VALVE_VOLTAGE_LIMIT; pos_plus_end = pos.sen; // CAN_TX_PRES((int16_t) (V_out), (int16_t) (7)); } else if(VALVE_FR_timer < (int) (2.0 * (double) TMR_FREQ_5k)) { - V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = -VALVE_VOLTAGE_LIMIT; } else if(VALVE_FR_timer == (int) (2.0 * (double) TMR_FREQ_5k)) { // CAN_TX_PRES((int16_t) (V_out), (int16_t) (8)); - V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = -VALVE_VOLTAGE_LIMIT; pos_minus_end = pos.sen; first_check = 1; VALVE_FR_timer = 0; @@ -1688,7 +1687,7 @@ VALVE_PWM_VALVE_DZ = V_out + VALVE_CENTER; // Output Voltage Linearization and Dead Zone Cancellation (Electrical dead-zone) - double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ/PWM_RESOL*SUPPLY_VOLTAGE*1000.0; // convert PWM duty to mV + double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ/5.0*1000.0; // convert PWM duty to mV double CUR_PWM_DZ = PWM_duty_byLT(CUR_PWM_nonlin); if (CUR_PWM_DZ > 0) V_out = (int)CUR_PWM_DZ + 143; @@ -1710,6 +1709,9 @@ if (flag_data_request[0] == HIGH) { //position+velocity CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen); + //CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0); + //CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]); + //pc.printf("can good"); // CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR); } @@ -1750,7 +1752,7 @@ //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM)); - // CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE * PWM_RESOL / SUPPLY_VOLTAGE)); + // CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE / SUPPLY_VOLTAGE)); // CAN_TX_VALVE_POSITION((int16_t) P_GAIN_JOINT_POSITION); // CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos); // CAN_TX_VALVE_POSITION((int16_t) flag_flowrate); @@ -1780,7 +1782,7 @@ /******************************************************* *** PWM ********************************************************/ - PWM_out= V_out/12000.0; // Full duty : 12000.0mV + PWM_out= V_out/SUPPLY_VOLTAGE; // 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 @@ -1824,10 +1826,8 @@ // LED = 1; // LED != LED; // pc.printf("A %f\n", (double) pres_A.sen); -// pc.printf("A %f\n", PRES_SENSOR_A_PULSE_PER_BAR); -// pc.printf("B %f\n", (double) pres_B.sen); -// pc.printf("B %f\n", PRES_SENSOR_B_PULSE_PER_BAR); -// pc.printf("preAVref %d\n", (int) PRES_A_VREF); +// CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[j]); +// j++; } /*******************************************************