Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2_
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 18:b8adf1582ea3
- Parent:
- 17:1865016ca2e7
- Child:
- 19:23b7c1ad8683
--- 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++;
}
/*******************************************************