Seunghoon Shin
20210305
Diff: main.cpp
- Revision:
- 227:699c3e572283
- Parent:
- 226:82a3ca333004
--- a/main.cpp Wed Mar 03 10:56:25 2021 +0000
+++ b/main.cpp Thu Mar 11 07:27:23 2021 +0000
@@ -79,13 +79,13 @@
extern int CID_RX_CMD;
extern int CID_RX_REF_POSITION;
-extern int CID_RX_REF_VALVE_POS;
+extern int CID_RX_REF_OPENLOOP;
extern int CID_RX_REF_PWM;
extern int CID_TX_INFO;
-extern int CID_TX_POSITION;
-extern int CID_TX_TORQUE;
-extern int CID_TX_PRES;
+extern int CID_TX_POS_VEL_TORQ;
+extern int CID_TX_PWM;
+extern int CID_TX_CURRENT;
extern int CID_TX_VOUT;
extern int CID_TX_VALVE_POSITION;
extern int CID_TX_SOMETHING;
@@ -93,6 +93,13 @@
float pres_A_new = 0.0f;
float pres_B_new = 0.0f;
+
+
+float temp_P_GAIN = 0.0f;
+float temp_I_GAIN = 0.0f;
+int temp_VELOCITY_COMP_GAIN = 0;
+
+
/*******************************************************************************
* REFERENCE MODE
******************************************************************************/
@@ -213,12 +220,17 @@
// make_delay();
// spi init
- enc_cs = 1;
+ eeprom_cs = 1;
eeprom.format(8,3);
eeprom.frequency(5000000); //5M
+ eeprom_cs = 0;
+ make_delay();
+
+ enc_cs = 1; //sw add
enc.format(8,0);
- enc.frequency(5000000); //5M
- enc_cs = 0;
+ enc.frequency(5000000); //10M
+ enc_cs = 0; //sw add
+
make_delay();
@@ -302,10 +314,10 @@
// timer_while ++;
//i2c for SW valve
- if(OPERATING_MODE == 5) {
- read_field(i2c_slave_addr1);
- if(DIR_VALVE_ENC < 0) value = 1023 - value;
- }
+ //if(OPERATING_MODE == 5) {
+// read_field(i2c_slave_addr1);
+// if(DIR_VALVE_ENC < 0) value = 1023 - value;
+// }
}
}
@@ -526,6 +538,22 @@
}
+// Reference Loop ==========================================================
+ switch (REFERENCE_MODE) {
+ case MODE_REF_NO_ACT: {
+ break;
+ }
+
+ case MODE_REF_DIRECT: {
+ pos.ref = REF_POSITION;
+ vel.ref = REF_VELOCITY;
+ torq.ref = REF_TORQUE;
+ break;
+ }
+ default:
+ break;
+ }
+
int UTILITY_MODE = 0;
int CONTROL_MODE = 0;
@@ -577,6 +605,7 @@
case MODE_FIND_HOME: {
if (FINDHOME_STAGE == FINDHOME_INIT) {
+ REFERENCE_MODE=MODE_REF_NO_ACT;
cnt_findhome = 0;
cnt_vel_findhome = 0;
pos.ref = pos.sen;
@@ -633,50 +662,11 @@
vel.ref = 0.0f;
alpha_trans = 0.0f;
- double torq_ref = 0.0f;
pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
CONTROL_MODE = MODE_JOINT_CONTROL;
- if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
-
- double I_REF_POS = 0.0f;
- double I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
- double I_REF_VC = 0.0f; // I_REF for velocity compensation
-
- double temp_vel_pos = 0.0f;
- double temp_vel_torq = 0.0f;
- double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
-
- if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
- temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * 3.14159f / 180.0f; // rad/s
- // L when P-gain = 100, f_cut = 10Hz L feedforward velocity
- } else if ((OPERATING_MODE & 0x01) == 1) {
- temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s
- // L when P-gain = 100, f_cut = 10Hz L feedforward velocity
- }
- if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
- else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
-
- I_REF = I_REF_POS;
-
- } else {
- float VALVE_POS_RAW_FORCE_FB = 0.0f;
- VALVE_POS_RAW_FORCE_FB = DDV_JOINT_POS_FF(vel.sen) + (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref));
-
- if (VALVE_POS_RAW_FORCE_FB >= 0) {
- valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
- } else {
- valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS;
- }
-
- VALVE_POS_CONTROL(valve_pos.ref);
-
- V_out = (float) Vout.ref;
-
- }
-
cnt_findhome++;
if (cnt_findhome >= T_move) {
cnt_findhome = 0;
@@ -686,83 +676,13 @@
vel.ref_home_pos = 0.0f;
FINDHOME_STAGE = FINDHOME_INIT;
CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
+ REFERENCE_MODE=MODE_REF_DIRECT;
}
}
break;
}
-// case MODE_VALVE_GAIN_SETTING: {
-// if (TMR3_COUNT_FLOWRATE == 0) {
-// if (pos_plus_end == pos_minus_end) need_enc_init = true;
-// else {
-// 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 * 1000.0f;
-// pos_plus_end = pos.sen;
-// } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) {
-// V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
-// pos_minus_end = pos.sen;
-// } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) {
-// need_enc_init = false;
-// check_vel_pos_init = (int) (0.9f * (float) (pos_plus_end - pos_minus_end));
-// check_vel_pos_fin = (int) (0.95f * (float) (pos_plus_end - pos_minus_end));
-// check_vel_pos_interv = check_vel_pos_fin - check_vel_pos_init;
-// }
-// temp_time = TMR_FREQ_5k;
-// }
-// TMR3_COUNT_FLOWRATE++;
-// if (TMR3_COUNT_FLOWRATE > temp_time) {
-// if (flag_flowrate % 2 == 0) { // (+)
-// VALVE_VOLTAGE = 1000.0f * (float) (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) {
-// VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0 * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // 0.9587=6*pi/65536*10000 0.5757=0.02525*0.02*0.0095*2*60*1000
-// // VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // PULSE/sec
-// fl_temp_cnt2++;
-// }
-// } else if (flag_flowrate % 2 == 1) { // (-)
-// VALVE_VOLTAGE = -1. * (float) (flag_flowrate / 2 + 1);
-// 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) {
-// VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0f * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE);
-// // VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE); // PULSE/sec
-// fl_temp_cnt2++;
-// }
-// }
-// if (fl_temp_cnt2 == 100) {
-//
-//
-//
-// //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0f));
-// cur_vel_sum = 0;
-// fl_temp_cnt = 0;
-// fl_temp_cnt2 = 0;
-// flag_flowrate++;
-// }
-// if (flag_flowrate == 10) {
-// V_out = 0;
-// flag_flowrate = 0;
-// TMR3_COUNT_FLOWRATE = 0;
-// valve_gain_repeat_cnt++;
-// if (valve_gain_repeat_cnt >= 1) {
-// CONTROL_MODE = MODE_NO_ACT;
-// valve_gain_repeat_cnt = 0;
-// }
-//
-// }
-// break;
-// }
-//
-// }
case MODE_PRESSURE_SENSOR_NULLING: {
// DAC Voltage reference set
if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
@@ -815,397 +735,92 @@
break;
}
-// case MODE_PRESSURE_SENSOR_CALIB: {
-// if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
-// 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 * 1000.0f;
-// 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;
-// V_out = 0;
-// PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_A_NULL;
-// PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
-// PRES_SENSOR_B_PULSE_PER_BAR = CUR_PRES_B_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_B_NULL;
-// PRES_SENSOR_B_PULSE_PER_BAR = PRES_SENSOR_B_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
-// CUR_PRES_A_sum = 0;
-// CUR_PRES_B_sum = 0;
-// CUR_PRES_A_mean = 0;
-// CUR_PRES_B_mean = 0;
-//
-//
-//
-// //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
-// //spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
-// }
-// TMR3_COUNT_PRES_CALIB++;
-// break;
-// }
+ case MODE_DDV_DEADZONE_AND_CENTER: {
+ if (FINDDZ_STAGE == FINDDZ_INIT) {
+ cnt_finddz = 0;
+ cnt_vel_finddz = 0;
+ temp_pos_ref = pos.sen;
+ temp_pos_ref_offset = 0.0f;
+ vel.ref = 0.0f;
+ FINDDZ_STAGE = FINDDZ_START1;
+ flag_finddz = 1;
+ FINDDZ_POSITION = pos.sen;
+ FINDDZ_POSITION_OLD = FINDDZ_POSITION;
+
+ temp_P_GAIN = P_GAIN_JOINT_POSITION;
+ temp_I_GAIN = I_GAIN_JOINT_POSITION;
+ temp_VELOCITY_COMP_GAIN = VELOCITY_COMP_GAIN;
-// 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 * 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;
-// V_out = 0.0f;
-// CONTROL_MODE = MODE_NO_ACT;
-// }
-// break;
-// }
-
- case MODE_DDV_POS_VS_PWM_ID: {
- CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
- VALVE_ID_timer = VALVE_ID_timer + 1;
+ P_GAIN_JOINT_POSITION = 1.0f;
+ I_GAIN_JOINT_POSITION = 0.0f;
+ VELOCITY_COMP_GAIN = 0;
+ FLAG_VALVE_DEADZONE = 0;
+ alpha_trans = 0.0f;
+ REFERENCE_MODE=MODE_REF_NO_ACT;
- if(VALVE_ID_timer < TMR_FREQ_5k*1) {
- Vout.ref = 3000.0f * sin(2.0f*3.14f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f);
- } else if(VALVE_ID_timer < TMR_FREQ_5k*2) {
- Vout.ref = 1000.0f*(ID_index_array[ID_index]);
- } else if(VALVE_ID_timer == TMR_FREQ_5k*2) {
- VALVE_POS_TMP = 0;
- data_num = 0;
- } else if(VALVE_ID_timer < TMR_FREQ_5k*3) {
- data_num = data_num + 1;
- VALVE_POS_TMP = VALVE_POS_TMP + value;
- } else if(VALVE_ID_timer == TMR_FREQ_5k*3) {
- Vout.ref = 0.0f;
- } else {
- VALVE_POS_AVG[ID_index] = VALVE_POS_TMP / data_num;
- VALVE_ID_timer = 0;
- ID_index= ID_index +1;
- }
+ } else if (FINDDZ_STAGE == FINDDZ_START1) {
+ cnt_finddz++;
+ CONTROL_MODE = MODE_JOINT_CONTROL;
+ FINDDZ_POSITION = pos.sen;
+ FINDDZ_VELOCITY = FINDDZ_POSITION - FINDDZ_POSITION_OLD;
+ FINDDZ_POSITION_OLD = FINDDZ_POSITION;
- if(ID_index>=25) {
- int i;
- VALVE_POS_AVG_OLD = VALVE_POS_AVG[0];
- for(i=0; i<25; i++) {
- VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]);
- if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) {
- VALVE_MAX_POS = VALVE_POS_AVG[i];
- VALVE_POS_AVG_OLD = VALVE_MAX_POS;
- } else if(VALVE_POS_AVG[i] < VALVE_POS_AVG_OLD) {
- VALVE_MIN_POS = VALVE_POS_AVG[i];
- VALVE_POS_AVG_OLD = VALVE_MIN_POS;
- }
- }
- spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS);
- spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS);
- for(int i=0; i<25; i++) {
- spi_eeprom_write(RID_VALVE_POS_VS_PWM_0 + i, (int16_t) VALVE_POS_VS_PWM[i]);
- }
- ID_index = 0;
- CONTROL_UTILITY_MODE = MODE_NO_ACT;
- }
+ if (temp_P_GAIN > 0.0f) temp_pos_ref_offset = temp_pos_ref_offset - (float)FINDDZ_VELOCITY*temp_P_GAIN;
+ else temp_pos_ref_offset = temp_pos_ref_offset - (float)FINDDZ_VELOCITY*20.0f;
+
+ pos.ref = temp_pos_ref + temp_pos_ref_offset;
+
+ if (cnt_finddz > 10000) FINDDZ_STAGE = FINDDZ_START2;
- break;
- }
+ } else if (FINDDZ_STAGE == FINDDZ_START2) {
+ CONTROL_MODE = MODE_JOINT_CONTROL;
+ FINDDZ_POSITION = pos.sen;
+ FINDDZ_VELOCITY = FINDDZ_POSITION - FINDDZ_POSITION_OLD;
+ FINDDZ_POSITION_OLD = FINDDZ_POSITION;
- case MODE_DDV_DEADZONE_AND_CENTER: {
- CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
- VALVE_DZ_timer = VALVE_DZ_timer + 1;
- if(first_check == 0) {
- if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
- Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
- } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
- Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
- pos_plus_end = pos.sen;
- } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
- Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
- } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
- Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
- pos_minus_end = pos.sen;
- } else if(VALVE_DZ_timer < (int) (3.0f * (float) TMR_FREQ_5k)) {
- Vout.ref = (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)) {
- Vout.ref = (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)) {
- Vout.ref = (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;
- data_num = 0;
-
- } else if(VALVE_DZ_timer < (int) (5.0f * (float) TMR_FREQ_5k)) {
- valve_pos.ref = DDV_POS_AVG;
- VALVE_POS_CONTROL(valve_pos.ref);
-
- } else if(VALVE_DZ_timer < (int) (6.0f * (float) TMR_FREQ_5k)) {
- valve_pos.ref = DDV_POS_AVG;
- VALVE_POS_CONTROL(valve_pos.ref);
-
- } else if(VALVE_DZ_timer == (int) (6.0f * (float) TMR_FREQ_5k)) {
- valve_pos.ref = DDV_POS_AVG;
- VALVE_POS_CONTROL(valve_pos.ref);
- FINAL_POS = pos.sen;
-
- if((FINAL_POS - START_POS)>200) {
- DZ_case = 1;
- } else if((FINAL_POS - START_POS)<-200) {
- DZ_case = -1;
- } else {
- DZ_case = 0;
+ if (flag_finddz > 0) {
+ temp_pos_ref = temp_pos_ref + 1.0f;
+ if (FINDDZ_VELOCITY > 0) {
+ cnt_vel_finddz = cnt_vel_finddz + 1;
+ }
+ if (cnt_vel_finddz > 5) {
+ VALVE_DEADZONE_PLUS = (int)(I_REF_fil/mA_PER_pulse);
+// temp_VALVE_DEADZONE_PLUS = temp_VALVE_DEADZONE_PLUS + I_REF_fil/mA_PER_pulse;
+ cnt_vel_finddz = 0;
+ flag_finddz = - flag_finddz - 1;
}
- CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6));
-
- first_check = 1;
- DZ_DIRECTION = 1;
- VALVE_DZ_timer = 0;
- Ref_Valve_Pos_Old = DDV_POS_AVG;
- DZ_NUM = 1;
- DZ_index = 1;
-
- }
- } 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)) {
- Vout.ref = (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)) {
- valve_pos.ref = Ref_Valve_Pos_Old - DZ_case * 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) {
- valve_pos.ref = VALVE_MAX_POS;
- }
- VALVE_POS_CONTROL(valve_pos.ref);
-
- } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
- Ref_Valve_Pos_Old = valve_pos.ref;
- FINAL_POS = pos.sen;
-
- if((FINAL_POS - START_POS)>100) {
- DZ_DIRECTION = 1 * DZ_case;
- } else if((FINAL_POS - START_POS)<-100) {
- DZ_DIRECTION = -1 * DZ_case;
- } else {
- DZ_DIRECTION = 1 * DZ_case;
- }
-
- VALVE_DZ_timer = 0;
- DZ_index= DZ_index *2;
- if(DZ_index >= 128) {
- FIRST_DZ = valve_pos.ref;
- DZ_NUM = 2;
- Ref_Valve_Pos_Old = FIRST_DZ;
- DZ_index = 1;
- DZ_DIRECTION = 1;
- }
+ } else if (flag_finddz < 0) {
+ temp_pos_ref = temp_pos_ref - 1.0f;
+ if (FINDDZ_VELOCITY < 0) {
+ cnt_vel_finddz = cnt_vel_finddz + 1;
}
- } 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)) {
- Vout.ref = (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)) {
- valve_pos.ref = Ref_Valve_Pos_Old - DZ_case * 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) {
- valve_pos.ref = VALVE_MAX_POS;
- }
- VALVE_POS_CONTROL(valve_pos.ref);
-
- } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
- Vout.ref = 0.0f;
- } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) {
- Ref_Valve_Pos_Old = valve_pos.ref;
- FINAL_POS = pos.sen;
-
- if((FINAL_POS - START_POS)>100) {
- DZ_DIRECTION = 1 * DZ_case;
- } else if((FINAL_POS - START_POS)<-100) {
- DZ_DIRECTION = -1 * DZ_case;
- } else {
- DZ_DIRECTION = -1 * DZ_case;
- }
-
- VALVE_DZ_timer = 0;
- DZ_index= DZ_index * 2;
- if(DZ_index >= 128) {
- SECOND_DZ = valve_pos.ref;
- VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
- first_check = 0;
- VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
- VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
-
- spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
- spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS);
- spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS);
-
- CONTROL_UTILITY_MODE = MODE_NO_ACT;
- DZ_index = 1;
- }
- }
- } else if(DZ_case == 0 && DZ_NUM ==1) {
- if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
- Vout.ref = (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)) {
- 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) {
- valve_pos.ref = VALVE_MAX_POS;
- }
- VALVE_POS_CONTROL(valve_pos.ref);
-
- } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
- Ref_Valve_Pos_Old = valve_pos.ref;
- FINAL_POS = pos.sen;
-
- if((FINAL_POS - START_POS)>100) {
- DZ_DIRECTION = 1;
- } else if((FINAL_POS - START_POS)<-100) {
- DZ_DIRECTION = -1;
- } else {
- DZ_DIRECTION = 1;
- }
- VALVE_DZ_timer = 0;
- DZ_index= DZ_index *2;
- if(DZ_index >= 128) {
- FIRST_DZ = valve_pos.ref;
- DZ_NUM = 2;
- Ref_Valve_Pos_Old = FIRST_DZ;
- DZ_index = 1;
- DZ_DIRECTION = 1;
- }
- }
- } else {
- if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
- Vout.ref = (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)) {
- 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) {
- valve_pos.ref = VALVE_MAX_POS - 1;
- }
- VALVE_POS_CONTROL(valve_pos.ref);
-
- } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
- Vout.ref = 0.0f;
- } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) {
- Ref_Valve_Pos_Old = valve_pos.ref;
- FINAL_POS = pos.sen;
-
- if((FINAL_POS - START_POS)>100) {
- DZ_DIRECTION = 1;
- } else if((FINAL_POS - START_POS)<-100) {
- DZ_DIRECTION = -1;
- } else {
- DZ_DIRECTION = 1;
- }
-
- VALVE_DZ_timer = 0;
- DZ_index= DZ_index *2;
- if(DZ_index >= 128) {
- SECOND_DZ = valve_pos.ref;
- VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
- first_check = 0;
- VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
- VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
-
- spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
- spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS);
- spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS);
-
- CONTROL_UTILITY_MODE = MODE_NO_ACT;
- DZ_index = 1;
- }
+ if (cnt_vel_finddz > 5) {
+ VALVE_DEADZONE_MINUS = (int)(I_REF_fil/mA_PER_pulse);
+// temp_VALVE_DEADZONE_MINUS = temp_VALVE_DEADZONE_MINUS + I_REF_fil/mA_PER_pulse;
+ cnt_vel_finddz = 0;
+ flag_finddz = - flag_finddz + 1;
}
}
- }
- break;
- }
-
- case MODE_DDV_POS_VS_FLOWRATE: {
- CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
- VALVE_FR_timer = VALVE_FR_timer + 1;
- if(first_check == 0) {
- if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
- Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
- } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
- Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
- pos_plus_end = pos.sen;
- } else if(VALVE_FR_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
- Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
- } else if(VALVE_FR_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
- Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
- pos_minus_end = pos.sen;
- first_check = 1;
- VALVE_FR_timer = 0;
- valve_pos.ref = (float) VALVE_CENTER;
- ID_index = 0;
- max_check = 0;
- min_check = 0;
+ if (abs(flag_finddz) >= 6) {
+ FINDDZ_STAGE = FINDDZ_STOP;
+// VALVE_DEADZONE_PLUS = (int)(temp_VALVE_DEADZONE_PLUS / (double)flag_finddz * 2.0f);
+// VALVE_DEADZONE_MINUS = (int)(temp_VALVE_DEADZONE_MINUS / (double)flag_finddz * 2.0f);
+ spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
+ spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
}
- } else {
- if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
- pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
- CONTROL_MODE = MODE_JOINT_CONTROL;
- } 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]) + (float) VALVE_CENTER;
+ pos.ref = temp_pos_ref + temp_pos_ref_offset;
- VALVE_POS_CONTROL(valve_pos.ref);
- START_POS = pos.sen;
- } else if(VALVE_FR_timer < (int) (5.0f * (float) TMR_FREQ_5k)) {
- valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER;
- VALVE_POS_CONTROL(valve_pos.ref);
- data_num = data_num + 1;
- if(abs(0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen) > 20000.0f) {
- FINAL_POS = pos.sen;
- one_period_end = 1;
- }
- } else if(VALVE_FR_timer == (int) (5.0f * (float) TMR_FREQ_5k)) {
- FINAL_POS = pos.sen;
- one_period_end = 1;
- V_out = 0.0f;
- }
-
- if(one_period_end == 1) {
- if(valve_pos.ref > VALVE_MAX_POS) {
- max_check = 1;
- } else if(valve_pos.ref < VALVE_MIN_POS) {
- min_check = 1;
- }
- JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k; // pulse/sec
-
- VALVE_FR_timer = 0;
- one_period_end = 0;
- ID_index= ID_index +1;
- V_out = 0.0f;
- }
-
- if(max_check == 1 && min_check == 1) {
-
- VALVE_POS_NUM = ID_index;
- for(int i=0; i<100; i++) {
- spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0 + i, (int16_t) (JOINT_VEL[i] & 0xFFFF));
- spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0_1 + i, (int16_t) ((JOINT_VEL[i] >> 16) & 0xFFFF));
- }
- ID_index = 0;
- first_check = 0;
- VALVE_FR_timer = 0;
- CONTROL_UTILITY_MODE = MODE_NO_ACT;
- }
+ } else if (FINDDZ_STAGE == FINDDZ_STOP) {
+ FINDDZ_STAGE = FINDDZ_INIT;
+ CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
+ P_GAIN_JOINT_POSITION = temp_P_GAIN;
+ I_GAIN_JOINT_POSITION = temp_I_GAIN;
+ VELOCITY_COMP_GAIN = temp_VELOCITY_COMP_GAIN;
+ flag_finddz = 0;
+ FLAG_VALVE_DEADZONE = 1;
+ REFERENCE_MODE=MODE_REF_DIRECT;
}
break;
}
@@ -1254,23 +869,6 @@
}
break;
}
- case MODE_SEND_BUFFER: {
-// if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
-// CAN_TX_PRES((int16_t) (pos_array[cnt_send_buffer]), (int16_t) (ref_array[cnt_send_buffer])); // 1400
-// if(cnt_send_buffer>=buffer_data_size) {
-// CONTROL_UTILITY_MODE = MODE_FREQ_TEST;
-// }
-// cnt_send_buffer++;
-// }
-
- break;
- }
- case MODE_SEND_OVER: {
- CAN_TX_TORQUE((int16_t) (buffer_data_size)); //1300
- CONTROL_UTILITY_MODE = MODE_NO_ACT;
- CONTROL_MODE = MODE_NO_ACT;
- break;
- }
case MODE_STEP_TEST: {
float valve_pos_ref = 0.0f;
@@ -1324,7 +922,7 @@
} else if (CURRENT_CONTROL_MODE == 0) { //PWM
V_out = valve_pos.ref;
} else {
- I_REF = (double)(valve_pos.ref * (float)mA_PER_pulse);
+ I_REF = valve_pos.ref * 0.001f;
}
break;
}
@@ -1511,7 +1109,6 @@
float x_4_des_dot = (x_4_des - x_4_des_old)*(float) TMR_FREQ_5k;
x_4_des_old = x_4_des;
- float V_input = 0.0f;
V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*(-torq.err))/g4;
float rho_a = 0.00001f;
@@ -1605,15 +1202,6 @@
else if (CUR_PWM_lin < 0) V_out = (float) (CUR_PWM_lin - 174.0f);
else V_out = (float) (CUR_PWM_lin);
} else { //////////////////////////sw valve
- // Output Voltage Linearization
-// double CUR_PWM_nonlin = V_out; // Unit : mV
-// double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin); // -8000~8000
-
- // Dead Zone Cancellation (Electrical dead-zone)
-// if (CUR_PWM_lin > 0) V_out = (float) (CUR_PWM_lin + 169.0f);
-// else if (CUR_PWM_lin < 0) V_out = (float) (CUR_PWM_lin - 174.0f);
-// else V_out = (float) (CUR_PWM_lin);
-
// Output Voltage Linearization & Dead Zone Cancellation (Electrical dead-zone) by SW
if (V_out > 0 ) V_out = (V_out + 180.0f)/0.8588f;
else if (V_out < 0) V_out = (V_out - 200.0f)/0.8651f;
@@ -1667,51 +1255,21 @@
CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
}
}
-
}
-
- // ID:1300
+ // PWM, ID:1300
if (flag_data_request[1] == HIGH) {
- CAN_TX_TORQUE((int16_t) 7); //1300
+ CAN_TX_PWM((int16_t) (V_out)); //1300
}
- // Reference Current, Current Current (ID:1700)
+ // Reference Current, Current Current (ID:1400)
if (flag_data_request[2] == HIGH) {
-
- CAN_TX_SOMETHING((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t) (HOMEPOS_OFFSET), (int16_t) (0));
- //double t_value = 0.0f;
- // if(value>=(float) VALVE_CENTER) {
- // t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
- // } else {
- // t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
- // }
- // double t_value_ref = 0.0f;
- // if(valve_pos.ref>=(float) VALVE_CENTER) {
- // t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
- // } else {
- // t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
- // }
- //
- //
- // CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
+ CAN_TX_CURRENT((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse)); // 1400
}
-
- //If it doesn't rest, below can can not work.
- for (can_rest = 0; can_rest < 10000; can_rest++) {
- ;
- }
-
- // ID:1500
+
+ // Reference Current, Current Current (ID:1700)
if (flag_data_request[3] == HIGH) {
- //PWM
- CAN_TX_PWM((int16_t) (torq.ref)); //1500
- }
-
- // ID:1600
- if (flag_data_request[4] == HIGH) {
- //valve position
- CAN_TX_VALVE_POSITION((int16_t) (a_hat*0.0001f), (int16_t) 0, (int16_t) 0, (int16_t) 0); //1600
+ CAN_TX_SOMETHING((int16_t) (0), (int16_t) (0), (int16_t) (0), (int16_t) (0)); // 1700
}
TMR2_COUNT_CAN_TX = 0;