Yeseong Jeong
20210203
Diff: main.cpp
- Revision:
- 49:e7bcfc244d40
- Parent:
- 48:889798ff9329
- Child:
- 50:3c630b5eba9f
--- 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.);