Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2
rainbow
Revision 227:699c3e572283, committed 2021-03-11
- Comitter:
- hyhoon
- Date:
- Thu Mar 11 07:27:23 2021 +0000
- Parent:
- 226:82a3ca333004
- Child:
- 228:763bbefcb79f
- Commit message:
- 210311
Changed in this revision
--- a/CAN/function_CAN.cpp Wed Mar 03 10:56:25 2021 +0000 +++ b/CAN/function_CAN.cpp Thu Mar 11 07:27:23 2021 +0000 @@ -8,13 +8,13 @@ // CAN ID Setting Variables int CID_RX_CMD = 100; int CID_RX_REF_POSITION = 200; -int CID_RX_REF_VALVE_POS = 300; +int CID_RX_REF_OPENLOOP = 300; int CID_RX_REF_PWM = 400; int CID_TX_INFO = 1100; -int CID_TX_POSITION = 1200; -int CID_TX_TORQUE = 1300; -int CID_TX_PRES = 1400; +int CID_TX_POS_VEL_TORQ = 1200; +int CID_TX_PWM = 1300; +int CID_TX_CURRENT = 1400; int CID_TX_VOUT = 1500; int CID_TX_VALVE_POSITION = 1600; int CID_TX_SOMETHING = 1700; @@ -39,13 +39,13 @@ CID_RX_CMD = (int) (BNO + INIT_CID_RX_CMD); CID_RX_REF_POSITION = (int) (BNO + INIT_CID_RX_REF_POSITION); - CID_RX_REF_VALVE_POS = (int) (BNO + INIT_CID_RX_REF_VALVE_POS); + CID_RX_REF_OPENLOOP = (int) (BNO + INIT_CID_RX_REF_OPENLOOP); CID_RX_REF_PWM = (int) (BNO + INIT_CID_RX_REF_PWM); CID_TX_INFO = (int) (BNO + INIT_CID_TX_INFO); - CID_TX_POSITION = (int) (BNO + INIT_CID_TX_POSITION); - CID_TX_TORQUE = (int) (BNO + INIT_CID_TX_TORQUE); - CID_TX_PRES = (int) (BNO + INIT_CID_TX_PRES); + CID_TX_POS_VEL_TORQ = (int) (BNO + INIT_CID_TX_POS_VEL_TORQ); + CID_TX_PWM = (int) (BNO + INIT_CID_TX_PWM); + CID_TX_CURRENT = (int) (BNO + INIT_CID_TX_CURRENT); CID_TX_VOUT = (int) (BNO + INIT_CID_TX_VOUT); CID_TX_VALVE_POSITION = (int) (BNO + INIT_CID_TX_VALVE_POSITION); CID_TX_SOMETHING = (int) (BNO + INIT_CID_TX_SOMETHING); @@ -140,6 +140,7 @@ // if (flag_data_request[2] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC //if (flag_data_request[2] == HIGH) dac_2 = PRES_B_VREF/3.3; + break; } @@ -638,84 +639,54 @@ int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8); if((OPERATING_MODE&0b001)==0) { // Rotary Actuator - pos.ref = (double)temp_pos * 10.0f; - vel.ref = (double)temp_vel * 256.0f; + REF_POSITION = (double)temp_pos * 10.0f; + REF_VELOCITY = (double)temp_vel * 256.0f; } else { //Linear Actuator - pos.ref = (double)temp_pos * 10.0f; - vel.ref = (double)temp_vel * 256.0f; + REF_POSITION = (double)temp_pos * 10.0f; + REF_VELOCITY = (double)temp_vel * 256.0f; } - torq.ref = (double)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE; //N - torq.ref_diff = torq.ref - torq.ref_old; - torq_dot.sen = torq.sen-torq_sen_past; - torq.ref_old = torq.ref; - torq_sen_past = torq.sen; + REF_TORQUE = (double)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE; //N + torq.ref_diff = REF_TORQUE - REF_TORQUE_OLD; + REF_TORQUE_OLD = REF_TORQUE; if(CAN_FREQ == -1) { // Position, Velocity, and Torque (ID:1200) if (flag_data_request[0] == HIGH) { if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator if (SENSING_MODE == 0) { - CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen*10.0f)); + CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f)); } else if (SENSING_MODE == 1) { - CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*10.0f), (int16_t) ((pres_B.sen)*10.0f)); + CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); } } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator if (SENSING_MODE == 0) { CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); } else if (SENSING_MODE == 1) { - CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*10.0f), (int16_t) ((pres_B.sen)*10.0f)); + 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) (pos.ref/10.0f)); //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) (I_ERR / mA_PER_pulse)); - //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 (int can_rest2 = 0; can_rest2 < 10000; can_rest2++) { - ; - } - - // 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 } } - } else if(address==CID_RX_REF_VALVE_POS) { + } else if(address==CID_RX_REF_OPENLOOP) { int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8); if (((OPERATING_MODE&0b110)>>1) == 0) { //Moog Valve @@ -1336,7 +1307,7 @@ { CANMessage temp_msg; - temp_msg.id = CID_TX_POSITION; + temp_msg.id = CID_TX_POS_VEL_TORQ; temp_msg.len = 6; temp_msg.data[0] = (uint8_t) t_pos; temp_msg.data[1] = (uint8_t) (t_pos >> 8); @@ -1353,7 +1324,7 @@ CANMessage temp_msg; - temp_msg.id = CID_TX_POSITION; + temp_msg.id = CID_TX_POS_VEL_TORQ; temp_msg.len = 8; temp_msg.data[0] = (uint8_t) t_pos; temp_msg.data[1] = (uint8_t) (t_pos >> 8); @@ -1367,24 +1338,11 @@ can.write(temp_msg); } - -void CAN_TX_TORQUE(int16_t t_torque) +void CAN_TX_CURRENT(int16_t t_pres_a, int16_t t_pres_b) { CANMessage temp_msg; - temp_msg.id = CID_TX_TORQUE; - temp_msg.len = 2; - temp_msg.data[0] = (uint8_t) t_torque; - temp_msg.data[1] = (uint8_t) (t_torque >> 8); - - can.write(temp_msg); -} - -void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b) -{ - CANMessage temp_msg; - - temp_msg.id = CID_TX_PRES; + temp_msg.id = CID_TX_CURRENT; temp_msg.len = 4; temp_msg.data[0] = (uint8_t) t_pres_a; temp_msg.data[1] = (uint8_t) (t_pres_a >> 8); @@ -1398,7 +1356,7 @@ { CANMessage temp_msg; - temp_msg.id = CID_TX_VOUT; + temp_msg.id = CID_TX_PWM; temp_msg.len = 2; temp_msg.data[0] = (uint8_t) t_pwm; temp_msg.data[1] = (uint8_t) (t_pwm >> 8);
--- a/CAN/function_CAN.h Wed Mar 03 10:56:25 2021 +0000 +++ b/CAN/function_CAN.h Thu Mar 11 07:27:23 2021 +0000 @@ -9,13 +9,13 @@ // INIT_CID #define INIT_CID_RX_CMD 100 #define INIT_CID_RX_REF_POSITION 200 -#define INIT_CID_RX_REF_VALVE_POS 300 +#define INIT_CID_RX_REF_OPENLOOP 300 #define INIT_CID_RX_REF_PWM 400 #define INIT_CID_TX_INFO 1100 -#define INIT_CID_TX_POSITION 1200 -#define INIT_CID_TX_TORQUE 1300 -#define INIT_CID_TX_PRES 1400 +#define INIT_CID_TX_POS_VEL_TORQ 1200 +#define INIT_CID_TX_PWM 1300 +#define INIT_CID_TX_CURRENT 1400 #define INIT_CID_TX_VOUT 1500 #define INIT_CID_TX_VALVE_POSITION 1600 #define INIT_CID_TX_SOMETHING 1700 @@ -150,9 +150,9 @@ // Sensor & State Transmission void CAN_TX_POSITION_FT(int16_t t_pos, int16_t t_vel, int16_t t_torq); void CAN_TX_POSITION_PRESSURE(int16_t t_pos, int16_t t_vel, int16_t t_pa, int16_t t_pb); -//void CAN_TX_TORQUE(int16_t t_valve_pos, int16_t t_vout); -void CAN_TX_TORQUE(int16_t t_valve_pos); -void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b); +//void CAN_TX_PWM(int16_t t_valve_pos, int16_t t_vout); +void CAN_TX_PWM(int16_t t_valve_pos); +void CAN_TX_CURRENT(int16_t t_pres_a, int16_t t_pres_b); void CAN_TX_PWM(int16_t t_pwm); void CAN_TX_VALVE_POSITION(int16_t t_valve_pos_1, int16_t t_valve_pos_2, int16_t t_ref_valve_pos, int16_t t_pwm); void CAN_TX_SOMETHING (int16_t t_a, int16_t t_b, int16_t t_c, int16_t t_d);
--- a/function_utilities/function_utilities.cpp Wed Mar 03 10:56:25 2021 +0000 +++ b/function_utilities/function_utilities.cpp Thu Mar 11 07:27:23 2021 +0000 @@ -17,7 +17,7 @@ uint8_t CONTROL_UTILITY_MODE = 0; uint8_t CURRENT_CONTROL_MODE = 0; // (0 : pwm, 1 : current control) uint8_t FLAG_VALVE_DEADZONE = 0; -uint8_t REFERENCE_MODE = 0; +uint8_t REFERENCE_MODE = 1; int16_t CAN_FREQ = 500; int16_t DIR_JOINT_ENC = 0; int16_t DIR_VALVE = 0; @@ -113,10 +113,11 @@ float DAC_REF; float DAC_RESOL; -int REF_POSITION; -int REF_VELOCITY; -int16_t REF_TORQUE; -int16_t REF_PRES_DIFF; +float REF_POSITION; +float REF_VELOCITY; +float REF_TORQUE; +float REF_TORQUE_OLD; +float REF_PRES_DIFF; int16_t REF_PWM; int16_t REF_VALVE_POSITION; int16_t REF_CURRENT; @@ -287,6 +288,20 @@ int FINDHOME_POSITION = 0; int FINDHOME_POSITION_OLD = 0; +int cnt_finddz = 0; +int cnt_vel_finddz = 0; +int flag_finddz = 0; +int FINDDZ_VELOCITY = 0; +int FINDDZ_VELOCITY_OLD = 0; +int FINDDZ_POSITION = 0; +int FINDDZ_POSITION_OLD = 0; + +double temp_VALVE_DEADZONE_PLUS = 0.0f; +double temp_VALVE_DEADZONE_MINUS = 0.0f; +float temp_pos_ref = 0.0f; +float temp_pos_ref_offset = 0.0f; + + // valve gain int check_vel_pos_init = 0; int check_vel_pos_fin = 0; @@ -328,6 +343,12 @@ int FINDHOME_GOTOLIMIT = 1; int FINDHOME_ZEROPOSE = 2; +int FINDDZ_STAGE = 0; +int FINDDZ_INIT = 0; +int FINDDZ_START1 = 1; +int FINDDZ_START2 = 2; +int FINDDZ_STOP = 3; + float alpha_trans = 0.0f; float V_out=0.0f; @@ -389,7 +410,6 @@ for (i = 0; i < 1000000; i++) { ; } - }
--- 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;
--- a/setting.h Wed Mar 03 10:56:25 2021 +0000 +++ b/setting.h Thu Mar 11 07:27:23 2021 +0000 @@ -9,6 +9,7 @@ #define PWM_ARR 0x1194 // loop 20k, pwm 10k //#define PWM_ARR 0x2328 // loop 10k, pwm 5k #define TMR3_COUNT 0x4650 // loop 5k +//#define TMR3_COUNT 0x2328 // loop 10k #define TMR2_COUNT 0x2710 // loop 500hz with prescale 18 #define FREQ_500 500.0f @@ -208,10 +209,11 @@ extern float DAC_REF; extern float DAC_RESOL; -extern int REF_POSITION; -extern int REF_VELOCITY; -extern int16_t REF_TORQUE; -extern int16_t REF_PRES_DIFF; +extern float REF_POSITION; +extern float REF_VELOCITY; +extern float REF_TORQUE; +extern float REF_TORQUE_OLD; +extern float REF_PRES_DIFF; extern int16_t REF_PWM; extern int16_t REF_VALVE_POSITION; extern int16_t REF_CURRENT; @@ -376,6 +378,19 @@ extern int FINDHOME_POSITION; extern int FINDHOME_POSITION_OLD; +extern int cnt_finddz; +extern int cnt_vel_finddz; +extern int flag_finddz; +extern int FINDDZ_VELOCITY; +extern int FINDDZ_VELOCITY_OLD; +extern int FINDDZ_POSITION; +extern int FINDDZ_POSITION_OLD; + +extern double temp_VALVE_DEADZONE_PLUS; +extern double temp_VALVE_DEADZONE_MINUS; +extern float temp_pos_ref; +extern float temp_pos_ref_offset; + // valve gain extern int check_vel_pos_init; extern int check_vel_pos_fin; @@ -418,6 +433,12 @@ extern int FINDHOME_GOTOLIMIT; extern int FINDHOME_ZEROPOSE; +extern int FINDDZ_STAGE; +extern int FINDDZ_INIT; +extern int FINDDZ_START1; +extern int FINDDZ_START2; +extern int FINDDZ_STOP; + extern float alpha_trans;