Kim GiJeong
/
HydraulicControlBoard_LIGHT_GJ
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.
Revision 56:6f50d9d3bfee, committed 2020-03-03
- Comitter:
- Lightvalve
- Date:
- Tue Mar 03 12:27:53 2020 +0000
- Parent:
- 55:b25725257569
- Child:
- 57:f4819de54e7a
- Commit message:
- 200303
Changed in this revision
--- a/CAN/function_CAN.cpp Wed Feb 26 12:51:52 2020 +0000
+++ b/CAN/function_CAN.cpp Tue Mar 03 12:27:53 2020 +0000
@@ -102,7 +102,10 @@
case CRX_SET_CAN_FREQ: {
CAN_FREQ = (int16_t) (msg.data[1] | msg.data[2] << 8);
+
ROM_RESET_DATA();
+
+ //spi_eeprom_write(RID_CAN_FREQ, (int16_t) CAN_FREQ);
break;
}
@@ -113,8 +116,7 @@
}
case CRX_SET_CONTROL_MODE: {
- //CONTROL_MODE = (int16_t) (msg.data[1]);
- CONTROL_UTILITY_MODE = (int16_t) (msg.data[1]);
+ CONTROL_MODE = (int16_t) (msg.data[1]);
if (CONTROL_MODE == 22) { //MODE_FIND_HOME
FLAG_FIND_HOME = true;
}
@@ -228,8 +230,8 @@
}
case CRX_SET_HOMEPOS: {
- //CONTROL_MODE = 22;
- CONTROL_UTILITY_MODE = 22;
+ CONTROL_MODE = 22;
+ SETTING_SWITCH = (int16_t) (msg.data[1]);
break;
}
@@ -283,6 +285,10 @@
VALVE_DEADZONE_MINUS = (int16_t) (msg.data[5] | msg.data[6] << 8);
ROM_RESET_DATA();
+
+ //spi_eeprom_write(RID_VALVE_CNETER, (int16_t) (VALVE_CENTER));
+ //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS));
+ //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS));
break;
}
@@ -443,9 +449,11 @@
}
case CRX_SET_VALVE_LIMIT: {
- VALVE_MIN_POS = (int16_t) (msg.data[1] | msg.data[2] << 8);
- VALVE_MAX_POS = (int16_t) (msg.data[3] | msg.data[4] << 8);
+ VALVE_LIMIT_MINUS = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ VALVE_LIMIT_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8);
ROM_RESET_DATA();
+ //spi_eeprom_write(RID_VALVE_LIMIT_MINUS, (int16_t) VALVE_LIMIT_MINUS);
+ //spi_eeprom_write(RID_VALVE_LIMIT_PLUS, (int16_t) VALVE_LIMIT_PLUS);
break;
}
@@ -627,8 +635,9 @@
break;
}
case CRX_SET_VALVE_CENTER_OFFSET: {
- VALVE_CENTER_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
- VALVE_CENTER = VALVE_CENTER + VALVE_CENTER_OFFSET;
+ VALVE_CENTER_OFFSET_times10 = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ VALVE_CENTER_OFFSET = ((float) VALVE_CENTER_OFFSET_times10) * 0.1f;
+ DDV_CENTER = DDV_CENTER + VALVE_CENTER_OFFSET;
ROM_RESET_DATA();
break;
}
@@ -703,37 +712,22 @@
if(address==CID_RX_CMD){
unsigned int CMD = msg.data[0];
ReadCMD(CMD);
-
} else if(address==CID_RX_REF_POSITION) {
-
+ //int32_t temp_pos = (int32_t) (msg.data[0] | msg.data[1] << 8 | msg.data[2] << 16 | msg.data[3] << 24);
int16_t temp_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
+ //int32_t temp_vel = (int32_t) (msg.data[4] | msg.data[5] << 8 | msg.data[6] << 16 | msg.data[7] << 24);
int16_t temp_vel = (int16_t) (msg.data[2] | msg.data[3] << 8);
- 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 * 1.0f;
- vel.ref = (double)temp_vel * 10.0f;
- }
- else { //Linear Actuator
- pos.ref = (double)temp_pos * 4.0f;
- vel.ref = (double)temp_vel * 100.0f;
- }
-
+ int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8);
+ pos.ref = (double)temp_pos * 4.0f;
+ vel.ref = (double)temp_vel * 100.0f;
torq.ref = (double)temp_torq * 0.1f;
-
} else if(address==CID_RX_REF_VALVE_POS) {
int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
-
- if ((OPERATING_MODE&0b110) == 0) { //Moog Valve
- valve_pos.ref = (double) temp_ref_valve_pos;
- } else if ((OPERATING_MODE&0b110) == 1) { //KNR Valve
- valve_pos.ref = (double) temp_ref_valve_pos;
- } else { //SW Valve
- if(temp_ref_valve_pos >= 0) {
- valve_pos.ref = (double)VALVE_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_CENTER)/10000.0f;
- } else {
- valve_pos.ref = (double)VALVE_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_CENTER)/10000.0f;
- }
+ if(temp_ref_valve_pos >= 0){
+ valve_pos.ref = (double)DDV_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)DDV_CENTER)/10000.0f ;
+ }else{
+ valve_pos.ref = (double)DDV_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)DDV_CENTER)/10000.0f;
}
} else if(address==CID_RX_REF_PWM){
int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8);
@@ -755,7 +749,7 @@
temp_msg.data[2] = (uint8_t) CAN_FREQ;
temp_msg.data[3] = (uint8_t) (CAN_FREQ >> 8);
temp_msg.data[4] = (uint8_t) (flag_err[7] << 7 | flag_err[6] << 6 | flag_err[5] << 5 | flag_err[4] << 4 | flag_err[3] << 3 | flag_err[2] << 2 | flag_err[1] << 1 | flag_err[0]);
- temp_msg.data[5] = (uint8_t) CONTROL_UTILITY_MODE;
+ temp_msg.data[5] = (uint8_t) CONTROL_MODE;
temp_msg.data[6] = (uint8_t) OPERATING_MODE;
can.write(temp_msg);
@@ -804,7 +798,7 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 2;
temp_msg.data[0] = (uint8_t) CTX_SEND_CONTROL_MODE;
- temp_msg.data[1] = (uint8_t) CONTROL_UTILITY_MODE;
+ temp_msg.data[1] = (uint8_t) CONTROL_MODE;
can.write(temp_msg);
}
@@ -920,8 +914,8 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 7;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DEADZONE;
- temp_msg.data[1] = (uint8_t) VALVE_CENTER;
- temp_msg.data[2] = (uint8_t) (VALVE_CENTER >> 8);
+ temp_msg.data[1] = (uint8_t) (int) (VALVE_CENTER);
+ temp_msg.data[2] = (uint8_t) ((int) (VALVE_CENTER) >> 8);
temp_msg.data[3] = (uint8_t) (int) (VALVE_DEADZONE_PLUS);
temp_msg.data[4] = (uint8_t) ((int) (VALVE_DEADZONE_PLUS) >> 8);
temp_msg.data[5] = (uint8_t) (int) (VALVE_DEADZONE_MINUS);
@@ -1052,10 +1046,10 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_LIMIT;
- temp_msg.data[1] = (uint8_t) VALVE_MIN_POS;
- temp_msg.data[2] = (uint8_t) (VALVE_MIN_POS >> 8);
- temp_msg.data[3] = (uint8_t) VALVE_MAX_POS;
- temp_msg.data[4] = (uint8_t) (VALVE_MAX_POS >> 8);
+ temp_msg.data[1] = (uint8_t) VALVE_LIMIT_MINUS;
+ temp_msg.data[2] = (uint8_t) (VALVE_LIMIT_MINUS >> 8);
+ temp_msg.data[3] = (uint8_t) VALVE_LIMIT_PLUS;
+ temp_msg.data[4] = (uint8_t) (VALVE_LIMIT_PLUS >> 8);
can.write(temp_msg);
}
@@ -1135,8 +1129,8 @@
float temp_valve_deadzone_plus = 0.0f;
float temp_ddv_center = 0.0f;
- temp_valve_deadzone_plus = 10000.0f*((double)VALVE_DEADZONE_PLUS-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
- temp_valve_deadzone_minus = -10000.0f*((double)VALVE_DEADZONE_MINUS-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
+ temp_valve_deadzone_plus = 10000.0f*((double)VALVE_DEADZONE_PLUS-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
+ temp_valve_deadzone_minus = -10000.0f*((double)VALVE_DEADZONE_MINUS-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
temp_ddv_center = 0.0f;
temp_msg.id = CID_TX_INFO;
@@ -1215,10 +1209,10 @@
int16_t valve_pos_vs_pwm;
// valve_pos_vs_pwm = (int16_t) (VALVE_POS_VS_PWM[canindex]);
- if(VALVE_POS_VS_PWM[canindex]>= (float) VALVE_CENTER) {
- valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
+ if(VALVE_POS_VS_PWM[canindex]>=DDV_CENTER) {
+ valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
} else {
- valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
+ valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
}
int16_t PWM_VALVE_ID;
@@ -1240,12 +1234,12 @@
int32_t valve_pos_vs_flowrate;
valve_pos_vs_flowrate = (int32_t) (JOINT_VEL[canindex]);
- int16_t VALVE_POS_VALVE_ID = ID_index_array[canindex] * 10 + VALVE_CENTER;
+ int16_t VALVE_POS_VALVE_ID = ID_index_array[canindex] * 10 + (int) DDV_CENTER;
int16_t temp_valve_pos = 0;
- if(VALVE_POS_VALVE_ID>=VALVE_CENTER) {
- temp_valve_pos = (int16_t) (10000.0f*((double)VALVE_POS_VALVE_ID-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER));
+ if(VALVE_POS_VALVE_ID>=DDV_CENTER) {
+ temp_valve_pos = (int16_t) (10000.0f*((double)VALVE_POS_VALVE_ID-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER));
} else {
- temp_valve_pos = (int16_t) (-10000.0f*((double)VALVE_POS_VALVE_ID-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER));
+ temp_valve_pos = (int16_t) (-10000.0f*((double)VALVE_POS_VALVE_ID-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER));
}
temp_msg.id = CID_TX_INFO;
@@ -1351,7 +1345,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_PRES;
- temp_msg.len = 4;
+ temp_msg.len = 8;
temp_msg.data[0] = (uint8_t) t_pres_a;
temp_msg.data[1] = (uint8_t) (t_pres_a >> 8);
temp_msg.data[2] = (uint8_t) t_pres_b;
@@ -1363,8 +1357,8 @@
void CAN_TX_PWM(int16_t t_pwm) {
CANMessage temp_msg;
- temp_msg.id = CID_TX_VOUT;
- temp_msg.len = 2;
+ temp_msg.id = CID_TX_POSITION;
+ temp_msg.len = 8;
temp_msg.data[0] = (uint8_t) t_pwm;
temp_msg.data[1] = (uint8_t) (t_pwm >> 8);
@@ -1395,4 +1389,3 @@
-
--- a/SPI_EEP_ENC/SPI_EEP_ENC.h Wed Feb 26 12:51:52 2020 +0000 +++ b/SPI_EEP_ENC/SPI_EEP_ENC.h Tue Mar 03 12:27:53 2020 +0000 @@ -67,8 +67,8 @@ #define RID_STROKE 32 -//#define RID_VALVE_LIMIT_PLUS 34 -//#define RID_VALVE_LIMIT_MINUS 35 +#define RID_VALVE_LIMIT_PLUS 34 +#define RID_VALVE_LIMIT_MINUS 35 #define RID_ENC_PULSE_PER_POSITION 36 #define RID_TORQUE_SENSOR_PULSE_PER_TORQUE 37 @@ -88,7 +88,7 @@ #define RID_VALVE_MIN_POS 53 #define RID_VALVE_POS_NUM 54 -//#define RID_DDV_CENTER 55 +#define RID_DDV_CENTER 55 #define RID_VALVE_CENTER_OFFSET 56 #define RID_VALVE_GAIN_PLUS_1 60
--- a/function_utilities/function_utilities.cpp Wed Feb 26 12:51:52 2020 +0000
+++ b/function_utilities/function_utilities.cpp Tue Mar 03 12:27:53 2020 +0000
@@ -15,13 +15,15 @@
// Board Information
uint8_t BNO = 0;
uint8_t CONTROL_MODE = 0;
-uint8_t OPERATING_MODE = 0; // (00 : Moog & Rot, 01 : Moog & Lin, 10 : KNR & Rot, 11 : KNR & Lin, 101 : SW & Lin)
+uint8_t OPERATING_MODE = 0; // (00 : Moog & Rot, 01 : Moog & Lin, 10 : KNR & Rot, 11 : KNR & Lin)
uint8_t SENSING_MODE = 0; // (0 : torque, 1: pressure)
-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 SETTING_SWITCH = 0;
+uint8_t SETTING_SWITCH_OLD = 0;
uint8_t REFERENCE_MODE = 0;
int16_t CAN_FREQ = 500;
+int CAN_FREQUENCY = 500;
int16_t DIR_JOINT_ENC = 0;
int16_t DIR_VALVE = 0;
int16_t DIR_VALVE_ENC = 0;
@@ -82,8 +84,8 @@
int16_t STROKE;
-//int16_t VALVE_LIMIT_PLUS;
-//int16_t VALVE_LIMIT_MINUS;
+int16_t VALVE_LIMIT_PLUS;
+int16_t VALVE_LIMIT_MINUS;
float ENC_PULSE_PER_POSITION;
float TORQUE_SENSOR_PULSE_PER_TORQUE;
@@ -124,6 +126,7 @@
float CUR_TORQUE;
float CUR_PRES_A;
float CUR_PRES_B;
+int CUR_PWM;
int CUR_VALVE_POSITION;
unsigned int TMR2_COUNT_LED1;
@@ -142,20 +145,25 @@
int MODE_POS_FT_TRANS = 0;
-float CUR_CURRENT_mA = 0.0f;
-float CUR_PRES_A_BAR = 0.0f;
-float CUR_PRES_B_BAR = 0.0f;
-float CUR_TORQUE_NM = 0.0f;
-float CUR_TORQUE_NM_PRESS = 0.0f;
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////// SEUNGHOON ADD ///////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+
+float CUR_PRES_A_BAR;
+float CUR_PRES_B_BAR;
+float CUR_TORQUE_NM;
+float CUR_TORQUE_NM_PRESS;
-float PRES_A_VREF = 0.0f;
-float PRES_B_VREF = 0.0f;
-float TORQUE_VREF = 0.0f;
+float PRES_A_VREF;
+float PRES_A_VREF_TEST;
+float PRES_B_VREF;
+float PRES_B_VREF_TEST;
+float TORQUE_VREF;
-float VALVE_PWM_RAW_FB = 0.0f;
-float VALVE_PWM_RAW_FF = 0.0f;
-float VALVE_PWM_RAW = 0.0f;
-int VALVE_PWM_VALVE_DZ = 0;
+float VALVE_PWM_RAW_FB;
+float VALVE_PWM_RAW_FF;
+int VALVE_PWM_VALVE_DZ;
+int VALVE_INPUT_PWM;
float VALVE_GAIN_LPM_PER_V[10];
float VALVE_POS_VS_PWM[25];
@@ -163,25 +171,34 @@
int VALVE_MAX_POS;
int VALVE_MIN_POS;
+float DDV_CENTER;
int VALVE_POS_NUM;
float VALVE_CENTER_OFFSET;
float VALVE_DZ_MINUS_OFFSET;
float VALVE_DZ_PLUS_OFFSET;
+int VALVE_CENTER_OFFSET_times10;
-int TMR3_COUNT_FINDHOME = 0;
-int TMR3_COUNT_FLOWRATE = 0;
-int TMR3_COUNT_DEADZONE = 0;
-int TMR3_COUNT_PRES_NULL = 0;
-int TMR3_COUNT_TORQUE_NULL = 0;
-int TMR3_COUNT_PRES_CALIB = 0;
-int TMR3_COUNT_REFERENCE = 0;
-int TMR3_COUNT_JOINT = 0;
-int TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
+int TMR3_COUNT_FINDHOME;
+int TMR3_COUNT_FLOWRATE;
+int TMR3_COUNT_DEADZONE;
+int TMR3_COUNT_PRES_NULL;
+int TMR3_COUNT_TORQUE_NULL;
+int TMR3_COUNT_PRES_CALIB;
+int TMR3_COUNT_REFERENCE;
+int TMR3_COUNT_JOINT;
+int TMR3_COUNT_ROTARY_FRIC_TUNE;
-float TUNING_TIME = 600.0f; // sec
+bool FLAG_REFERENCE_VALVE_PWM;
+bool FLAG_REFERENCE_VALVE_POSITION;
+bool FLAG_REFERENCE_JOINT_POSITION;
+bool FLAG_REFERENCE_JOINT_TORQUE;
+bool FLAG_REFERENCE_PRES_DIFF;
+bool FLAG_REFERENCE_CURRENT;
-float REFERENCE_FREQ = 1.0f;
-float REFERENCE_MAG = 0.0f;
+float TUNING_TIME;
+
+float REFERENCE_FREQ;
+float REFERENCE_MAG;
bool FLAG_FIND_HOME;
@@ -264,8 +281,6 @@
int fl_temp_cnt2 = 0;
int cur_vel_sum = 0;
-float Cur_Valve_Open_pulse = 0.0f;
-
// find home
int CUR_VELOCITY_OLD = 0;
int cnt_findhome = 0;
@@ -284,20 +299,11 @@
float freq_fric_tune = 1.0f;
+bool FLAG_VALVE_OUTPUT_CALIB = false;
+
uint32_t TMR3_COUNT_CAN_TX = 0;
-// Current Control Variables
-double I_REF = 0.0f;
-double I_REF_fil = 0.0f;
-double I_ERR = 0.0f;
-double I_ERR_INT = 0.0f;
-double I_REF_fil_old = 0.0f;
-double I_REF_fil_diff = 0.0f;
-
-// system id
-int cnt_sysid = 0;
-double freq_sysid_Iref = 0.0f;
-
+float I_REF = 0.0f;
int TMR3_COUNT_IREF = 0;
float CUR_CURRENT = 0.0f;
float u_CUR[3] = {0.0f,0.0f,0.0f};
@@ -308,20 +314,6 @@
float alpha_trans = 0.0f;
-float V_out=0.0f;
-float V_rem=0.0f; // for anti-windup
-float V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
-
-float PWM_out=0.0f;
-
-double K_v = 0.0f; // valve flowrate gain
-double mV_PER_mA = 600.0f; // current >> voltage
-double mV_PER_pulse = 0.6f; // pulse >> voltage
-double mA_PER_pulse = 0.001f; // pulse >> current
-
-int timer_while = 0;
-int while_index = 0;
-
//int h1, h2, h3, h4, h5, h6;
/*******************************************************************************
@@ -408,8 +400,8 @@
writer.write(RID_ENC_LIMIT_MINUS,(int) ENC_LIMIT_MINUS);
writer.write(RID_ENC_LIMIT_PLUS,(int) ENC_LIMIT_PLUS);
writer.write(RID_STROKE,(int) STROKE);
- //writer.write(RID_VALVE_LIMIT_MINUS,(int) VALVE_LIMIT_MINUS);
- //writer.write(RID_VALVE_LIMIT_PLUS,(int) VALVE_LIMIT_PLUS);
+ writer.write(RID_VALVE_LIMIT_MINUS,(int) VALVE_LIMIT_MINUS);
+ writer.write(RID_VALVE_LIMIT_PLUS,(int) VALVE_LIMIT_PLUS);
writer.write(RID_ENC_PULSE_PER_POSITION,(int) (ENC_PULSE_PER_POSITION*10.0f));
writer.write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE,(int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 100.0f));
writer.write(RID_PRES_SENSOR_A_PULSE_PER_BAR,(int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
@@ -439,7 +431,7 @@
}
writer.write(RID_VALVE_MAX_POS, (int) VALVE_MAX_POS);
writer.write(RID_VALVE_MIN_POS, (int) VALVE_MIN_POS);
- //writer.write(RID_DDV_CENTER, (int) (DDV_CENTER * 10.0f));
+ writer.write(RID_DDV_CENTER, (int) (DDV_CENTER * 10.0f));
writer.write(RID_VALVE_POS_NUM, (int) VALVE_POS_NUM);
writer.write(RID_K_SPRING, (int) K_SPRING);
@@ -452,7 +444,6 @@
void ROM_CALL_DATA(void)
{
BNO = flashReadInt(Rom_Sector, RID_BNO);
- BNO = 0;
OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE);
SENSING_MODE = flashReadInt(Rom_Sector, RID_SENSING_MODE);
CURRENT_CONTROL_MODE = flashReadInt(Rom_Sector, RID_CURRENT_CONTROL_MODE);
@@ -489,8 +480,8 @@
ENC_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_MINUS);
ENC_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_PLUS);
STROKE = flashReadInt(Rom_Sector, RID_STROKE);
- //VALVE_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_MINUS);
- //VALVE_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_PLUS);
+ VALVE_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_MINUS);
+ VALVE_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_PLUS);
ENC_PULSE_PER_POSITION = (float) (flashReadInt(Rom_Sector, RID_ENC_PULSE_PER_POSITION)) * 0.1f;
// ENC_PULSE_PER_POSITION = (float) 1024.0f;
TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.01f;
@@ -524,7 +515,7 @@
}
VALVE_MAX_POS = flashReadInt(Rom_Sector, RID_VALVE_MAX_POS);
VALVE_MIN_POS = flashReadInt(Rom_Sector, RID_VALVE_MIN_POS);
- //DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f;
+ DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f;
VALVE_POS_NUM = flashReadInt(Rom_Sector, RID_VALVE_POS_NUM);
K_SPRING = flashReadInt(Rom_Sector, RID_K_SPRING);
@@ -561,8 +552,12 @@
ENC_pos_cur = spi_enc_read();
ENC_pos_diff = ENC_pos_cur - ENC_pos_old;
- //Kalman Filter
-// ENC_VEL_RAW = (int32_t) (ENC_pos_diff * TMR_FREQ_5k);
+// if (ENC_pos_diff > 1300 || ENC_pos_diff<-1300) {
+// //MOT_E_STOP(0);
+// }
+
+ ENC_VEL_RAW = (int32_t) (ENC_pos_diff * TMR_FREQ_5k);
+
// KF_Y_11 = ENC_pos_cur;
// KF_Y_21 = ENC_VEL_RAW;
// KF_X_11 = KF_G1_11 * KF_X_11 + KF_G1_12 * KF_X_21 + KF_G2_11 * KF_Y_11 + KF_G2_12*KF_Y_21;
@@ -570,17 +565,19 @@
// ENC_VEL_KF = (int32_t) KF_X_21;
//
// pos.sen = (DIR_JOINT_ENC) * ENC_pos_cur + enc_offset;
+// // CUR_POSITION = (DIR_JOINT_ENC) * ENC_pos_cur;
// vel.sen = (DIR_JOINT_ENC) * ENC_VEL_KF;
-
- //Low Pass Filter
-
+
+ // CUR_POSITION = ENC_pos_cur;
+ // CUR_VELOCITY = ENC_VEL_KF;
+
double NEW_POSITION = (double) ((DIR_JOINT_ENC) * ENC_pos_cur + enc_offset);
- double NEW_VELOCITY = (double) ((DIR_JOINT_ENC) * ENC_pos_diff * (int) FREQ_10k);
+ double NEW_VELOCITY = (double) ((DIR_JOINT_ENC) * ENC_pos_diff * (int) FREQ_5k);
- double alpha_update_pos = 1.0f/(1.0f + FREQ_10k/(2.0f*3.14f*100.0f));
+ double alpha_update_pos = 1.0f/(1.0f + FREQ_5k/(2.0f*3.14f*50.0f));
pos.sen = NEW_POSITION;
- vel.sen = (1.0f - alpha_update_pos) * vel.sen + alpha_update_pos * NEW_VELOCITY; // pulse/s
-
+ vel.sen = (1.0f - alpha_update_pos) * vel.sen + alpha_update_pos * NEW_VELOCITY;
+
ENC_pos_old = ENC_pos_cur;
}
@@ -604,4 +601,3 @@
-
--- a/main.cpp Wed Feb 26 12:51:52 2020 +0000
+++ b/main.cpp Tue Mar 03 12:27:53 2020 +0000
@@ -64,6 +64,15 @@
State INIT_Pos;
State INIT_torq;
+float V_out=0.0f;
+float V_rem=0.0f; // for anti-windup
+float V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
+
+float PWM_out=0.0f;
+
+int timer_while = 0;
+int while_index = 0;
+
extern int CID_RX_CMD;
extern int CID_RX_REF_POSITION;
extern int CID_RX_REF_VALVE_POS;
@@ -120,7 +129,7 @@
MODE_TORQUE_SENSOR_NULLING = 20, //20
MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21
MODE_FIND_HOME, //22
- MODE_VALVE_GAIN_SETTING, //23
+ MODE_VALVE_GAIN_SETTING, //23
MODE_PRESSURE_SENSOR_NULLING, //24
MODE_PRESSURE_SENSOR_CALIB, //25
MODE_ROTARY_FRICTION_TUNING, //26
@@ -128,7 +137,6 @@
MODE_DDV_POS_VS_PWM_ID = 30, //30
MODE_DDV_DEADZONE_AND_CENTER, //31
MODE_DDV_POS_VS_FLOWRATE, //32
- MODE_SYSTEM_ID, //33
};
int main()
@@ -136,7 +144,7 @@
/*********************************
*** Initialization
*********************************/
- LED = 1;
+ //LED = 1;
//pc.baud(9600);
// i2c init
@@ -172,9 +180,9 @@
make_delay();
// TMR2 init
- Init_TMR2();
- TIM2->CR1 ^= TIM_CR1_UDIS;
- make_delay();
+// Init_TMR2();
+// TIM2->CR1 ^= TIM_CR1_UDIS;
+// make_delay();
// CAN
can.attach(&CAN_RX_HANDLER);
@@ -183,7 +191,7 @@
//Timer priority
NVIC_SetPriority(TIM3_IRQn, 2);
- NVIC_SetPriority(TIM2_IRQn, 3);
+// NVIC_SetPriority(TIM2_IRQn, 3);
NVIC_SetPriority(TIM4_IRQn, 3);
//can.reset();
@@ -206,7 +214,7 @@
else
ID_index_array[i] = (i+1) * 0.5f;
}
-
+
/************************************
*** Program is operating!
*************************************/
@@ -234,15 +242,15 @@
if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <= max(JOINT_VEL[i],JOINT_VEL[i+1])) {
if(i==0) {
if(JOINT_VEL[i+1] == JOINT_VEL[i]) {
- Ref_Valve_Pos_FF = (float) VALVE_CENTER;
+ Ref_Valve_Pos_FF = DDV_CENTER;
} else {
- Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + (float) VALVE_CENTER;
+ Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
}
} else {
if(JOINT_VEL[i+1] == JOINT_VEL[i-1]) {
- Ref_Valve_Pos_FF = (float) VALVE_CENTER;
+ Ref_Valve_Pos_FF = DDV_CENTER;
} else {
- Ref_Valve_Pos_FF = ((float) 10*(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) * (REF_JOINT_VEL - JOINT_VEL[i-1])) + (float) VALVE_CENTER + (float) (10*ID_index_array[i-1]);
+ Ref_Valve_Pos_FF = ((float) 10*(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) * (REF_JOINT_VEL - JOINT_VEL[i-1])) + DDV_CENTER + (float) (10*ID_index_array[i-1]);
}
}
break;
@@ -254,7 +262,7 @@
Ref_Valve_Pos_FF = (float) VALVE_MIN_POS;
}
- Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - (float) VALVE_CENTER);
+ Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER);
return Ref_Valve_Pos_FF;
}
@@ -332,86 +340,82 @@
return PWM_duty;
}
-
-
-
-
/*******************************************************************************
TIMER INTERRUPT
*******************************************************************************/
float FREQ_TMR4 = (float)FREQ_20k;
float DT_TMR4 = (float)DT_20k;
-long CNT_TMR4 = 0;
-int TMR4_FREQ_10k = (int)FREQ_10k;
extern "C" void TIM4_IRQHandler(void)
{
+
if (TIM4->SR & TIM_SR_UIF ) {
/*******************************************************
*** Sensor Read & Data Handling
********************************************************/
- //Encoder
- if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) {
- ENC_UPDATE();
- }
+
+ //Using LoadCell
+// ADC1->CR2 |= 0x40000000; // adc _ 12bit
+// //while((ADC1->SR & 0b10));
+// float alpha_update_torque = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f));
+// float torque_new = ((float)ADC1->DR - PRES_A_NULL) / TORQUE_SENSOR_PULSE_PER_TORQUE + 1.0f;
+// torq.sen = torq.sen*(1.0f-alpha_update_torque)+torque_new*(alpha_update_torque);
+
+
//Pressure sensor A
ADC1->CR2 |= 0x40000000; // adc _ 12bit
//while((ADC1->SR & 0b10));
float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f));
+// float pres_A_new = ((float)ADC1->DR - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR;
float pres_A_new = ((float)ADC1->DR);
pres_A.sen = pres_A.sen*(1.0f-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
- torq.sen = - (pres_A.sen-2048.0f); //pulse -2047~2047 //SW just changed the sign to correct the direction of loadcell on LIGHT. Correct later.
+ torq.sen = - (pres_A.sen-2048.0f); //pulse -2047~2047
- //Pressure sensor B
- float alpha_update_pres_B = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f));
- float pres_B_new = ((float)ADC2->DR);
- pres_B.sen = pres_B.sen*(1.0f-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
+ //Pressure sensor 1B
+ //float alpha_update_pres_B = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f));
+ //float pres_B_new = ((float)ADC2->DR);
+ //pres_B.sen = pres_B.sen*(1.0f-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
//torq.sen = pres_A.sen * (float) PISTON_AREA_A - pres_B.sen * (float) PISTON_AREA_B;
+
//Current
//ADC3->CR2 |= 0x40000000; // adc _ 12bit
+// a1=ADC2->DR;
//int raw_cur = ADC3->DR;
//while((ADC3->SR & 0b10));
float alpha_update_cur = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*500.0f)); // f_cutoff : 500Hz
float cur_new = ((float)ADC3->DR-2048.0f)*20.0f/4096.0f; // unit : mA
cur.sen=cur.sen*(1.0f-alpha_update_cur)+cur_new*(alpha_update_cur);
//cur.sen = raw_cur;
-
- CNT_TMR4++;
+
+ /*******************************************************
+ *** Timer Counting & etc.
+ ********************************************************/
+ //CNT_TMR4++;
}
TIM4->SR = 0x0; // reset the status register
}
int j =0;
+//unsigned long CNT_TMR3 = 0;
+//float FREQ_TMR3 = (float)FREQ_5k;
float FREQ_TMR3 = (float)FREQ_5k;
float DT_TMR3 = (float)DT_5k;
+//float DT_TMR3 = (float)DT_1k;
int cnt_trans = 0;
double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
-int can_rest =0;
+int canfreq = CAN_FREQUENCY;
extern "C" void TIM3_IRQHandler(void)
{
if (TIM3->SR & TIM_SR_UIF ) {
-
-
-
- if ((OPERATING_MODE&0b110) == 0) {
- K_v = 0.4f; // Moog (LPM >> mA) , 100bar
- mV_PER_mA = 500.0f; // 5000mV/10mA
- mV_PER_pulse = 0.5f; // 5000mV/10000pulse
- mA_PER_pulse = 0.001f; // 10mA/10000pulse
- } else if ((OPERATING_MODE&0b110) == 1) {
- K_v = 0.5f; // KNR (LPM >> mA) , 100bar
- mV_PER_mA = 166.6666f; // 5000mV/30mA
- mV_PER_pulse = 0.5f; // 5000mV/10000pulse
- mA_PER_pulse = 0.003f; // 30mA/10000pulse
- }
+ ENC_UPDATE();
if(MODE_POS_FT_TRANS == 1) {
alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
@@ -434,26 +438,392 @@
}
- int UTILITY_MODE = 0;
- int CONTROL_MODE = 0;
-
- if (CONTROL_UTILITY_MODE >= 20 || CONTROL_UTILITY_MODE == 0) {
- UTILITY_MODE = CONTROL_UTILITY_MODE;
- CONTROL_MODE = MODE_NO_ACT;
- } else {
- CONTROL_MODE = CONTROL_UTILITY_MODE;
- UTILITY_MODE = MODE_NO_ACT;
- }
+
+ // CONTROL LOOP ------------------------------------------------------------
+
+ switch (CONTROL_MODE) {
+ case MODE_NO_ACT: {
+ V_out = 0.0f;
+ break;
+ }
+
+ case MODE_VALVE_POSITION_CONTROL: {
+ VALVE_POS_CONTROL(valve_pos.ref);
+ break;
+ }
+
+ case MODE_JOINT_CONTROL: {
+
+ float VALVE_POS_RAW_FORCE_FB = 0.0f;
+
+
+ double torq_ref = 0.0f;
+ //if(TMR3_COUNT_TEST % (int) (50) == 0){
+ pos.err = pos.ref - pos.sen; //[pulse]
+ vel.err = vel.ref - vel.sen; //[pulse/s]
+ torq_ref = torq.ref + (K_SPRING * pos.err * 0.01f + D_DAMPER * vel.err * 0.0001f) / ENC_PULSE_PER_POSITION; //[N]
+ //torq_ref_logging = torq_ref
+ //}
+
+ // torque feedback
+ torq.err = torq_ref - torq.sen; //[pulse]
+ torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse]
+// if (torq.err_sum > 1000) torq.err_sum = 1000;
+// if (torq.err_sum<-1000) torq.err_sum = -1000;
+
+
+ VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) /(float) TORQUE_SENSOR_PULSE_PER_TORQUE * 0.01f
+ + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err /(float) ENC_PULSE_PER_POSITION + 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;
+ }
+
+ if(I_GAIN_JOINT_TORQUE != 0){
+ double Ka = 1.0f / (double) I_GAIN_JOINT_TORQUE * (float) TORQUE_SENSOR_PULSE_PER_TORQUE * 100.0f;
+ if(valve_pos.ref>VALVE_MAX_POS){
+ double valve_pos_rem = valve_pos.ref - VALVE_MAX_POS;
+ valve_pos_rem = valve_pos_rem * Ka;
+ valve_pos.ref = VALVE_MAX_POS;
+ torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k;
+ }
+ else if(valve_pos.ref < VALVE_MIN_POS){
+ double valve_pos_rem = valve_pos.ref - VALVE_MIN_POS;
+ valve_pos_rem = valve_pos_rem * Ka;
+ valve_pos.ref = VALVE_MIN_POS;
+ torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k;
+ }
+ }
+
+ VALVE_POS_CONTROL(valve_pos.ref);
+
+ //TMR3_COUNT_TEST++;
+
+ break;
+ }
+
+
+ case MODE_VALVE_OPEN_LOOP: {
+ V_out = (float) Vout.ref;
+ break;
+ }
+
+ case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: {
+ float VALVE_POS_RAW_POS_FB = 0.0f; // Valve Position by Position Feedback
+ //float VALVE_POS_RAW_POS_FF = 0.0f; // Valve Position by Position Feedforward
+ float VALVE_POS_RAW_FORCE_FB = 0.0f; // Valve Position by Force Feedback
+ //int DDV_JOINT_CAN = 0;
+ // feedback input for position control
+ pos.err = pos.ref - (float) pos.sen;
+ pos.err_diff = pos.err - pos.err_old;
+ pos.err_old = pos.err;
+ pos.err_sum += pos.err;
+ if (pos.err_sum > 1000) pos.err_sum = 1000;
+ if (pos.err_sum<-1000) pos.err_sum = -1000;
+ VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err/(float) ENC_PULSE_PER_POSITION + (float) I_GAIN_JOINT_POSITION * 0.01f * pos.err_sum/(float) ENC_PULSE_PER_POSITION + (float) D_GAIN_JOINT_POSITION * pos.err_diff/(float) ENC_PULSE_PER_POSITION;
+
+
+ //Ref_Joint_Vel = Ref_Vel_Test;
+ // feedforward input for position control
+ // float Ref_Joint_Vel_Act = Ref_Joint_Vel/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+ // float K_ff = 0.9f;
+ // if(Ref_Joint_Vel_Act > 0) K_ff = 0.90f; // open
+ // if(Ref_Joint_Vel_Act > 0) K_ff = 0.75f; // close
+ // VALVE_POS_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50f;
+
+ //torque feedback
+ torq.err = - torq.ref + torq.sen;
+ torq.err_diff = torq.err - torq.err_old;
+ torq.err_old = torq.err;
+ torq.err_sum += torq.err;
+ if (torq.err_sum > 1000) torq.err_sum = 1000;
+ if (torq.err_sum<-1000) torq.err_sum = -1000;
+ VALVE_POS_RAW_FORCE_FB = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
+ VALVE_POS_RAW_FORCE_FB = VALVE_POS_RAW_FORCE_FB * 0.01f;
+
+// valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
+ valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref);
+
+ if (valve_pos.ref >= 0) {
+ valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS;
+ } else if(valve_pos.ref < 0) {
+ valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_MINUS;
+ }
+ VALVE_POS_CONTROL(valve_pos.ref);
-
- // UTILITY MODE ------------------------------------------------------------
-
- switch (UTILITY_MODE) {
- case MODE_NO_ACT: {
break;
}
+// case MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING: {
+//
+// break;
+// }
+
+// case MODE_JOINT_POSITION_PRES_CONTROL_PWM: {
+// pos.err = pos.ref - (float) pos.sen;
+// pos.err_diff = pos.err - pos.err_old;
+// pos.err_old = pos.err;
+// pos.err_sum += pos.err;
+// if (pos.err_sum > 1000) pos.err_sum = 1000;
+// if (pos.err_sum<-1000) pos.err_sum = -1000;
+// VALVE_PWM_RAW_POS = ((float) P_GAIN_JOINT_POSITION * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff)/(float) ENC_PULSE_PER_POSITION;
+//
+// torq.err = torq.ref - torq.sen;
+// torq.err_diff = torq.err - torq.err_old;
+// torq.err_old = torq.err;
+// torq.err_sum += torq.err;
+// if (torq.err_sum > 1000) torq.err_sum = 1000;
+// if (torq.err_sum<-1000) torq.err_sum = -1000;
+// VALVE_PWM_RAW_TORQ = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
+//
+// VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f;
+//
+// V_out = VALVE_PWM_RAW_POS + (float) COMPLIANCE_GAIN * 0.01f * VALVE_PWM_RAW_TORQ;
+//
+// CUR_FLOWRATE = (float) CUR_VELOCITY * 0.00009587f;
+// CUR_FLOWRATE = CUR_FLOWRATE * 0.5757f; // 0.4791=2*pi/65536*5000(pulse/tic to rad/s) 0.5757=0.02525*0.02*0.0095*2*60*1000 (radius * area * 2 * 60(sec --> min) * 1000(m^3 --> L))
+// if (DIR_VALVE > 0) {
+// if (CUR_FLOWRATE >= 0 && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[0]*1.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - 0.0f) / (VALVE_GAIN_LPM_PER_V[0]*1.0f - 0.0f) + 0.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[0]*1.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[2]*2.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[0]*1.0f) / (VALVE_GAIN_LPM_PER_V[2]*2.0f - VALVE_GAIN_LPM_PER_V[0]*1.) + 1.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[2]*2.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[4]*3.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[2]*2.0f) / (VALVE_GAIN_LPM_PER_V[4]*3.0f - VALVE_GAIN_LPM_PER_V[2]*2.) + 2.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[4]*3.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[6]*4.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[4]*3.0f) / (VALVE_GAIN_LPM_PER_V[6]*4.0f - VALVE_GAIN_LPM_PER_V[4]*3.) + 3.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[6]*4.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[6]*4.0f) / (VALVE_GAIN_LPM_PER_V[8]*5.0f - VALVE_GAIN_LPM_PER_V[6]*4.) + 4.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = 5.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) && CUR_FLOWRATE < 0.0f) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) / (0.0f - VALVE_GAIN_LPM_PER_V[1]*(-1.)) - 1.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[1]*(-1.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) / ((VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - VALVE_GAIN_LPM_PER_V[3]*(-2.)) - 2.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[3]*(-2.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) / ((VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - VALVE_GAIN_LPM_PER_V[5]*(-3.)) - 3.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[5]*(-3.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) / ((VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - VALVE_GAIN_LPM_PER_V[7]*(-4.)) - 4.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[7]*(-4.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) / ((VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - VALVE_GAIN_LPM_PER_V[9]*(-5.)) - 5.0f;
+// else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[9]*(-5.0f))) VALVE_FF_VOLTAGE = -5;
+// else VALVE_FF_VOLTAGE = 0;
+// } else {
+// if (CUR_FLOWRATE >= 0 && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[0]*1.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - 0.0f) / (VALVE_GAIN_LPM_PER_V[0]*1.0f - 0.0f) + 0.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[1]*1.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[2]*2.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[0]*1.0f) / (VALVE_GAIN_LPM_PER_V[2]*2.0f - VALVE_GAIN_LPM_PER_V[0]*1.0f) + 1.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[3]*2.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[4]*3.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[2]*2.0f) / (VALVE_GAIN_LPM_PER_V[4]*3.0f - VALVE_GAIN_LPM_PER_V[2]*2.0f) + 2.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[5]*3.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[6]*4.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[4]*3.0f) / (VALVE_GAIN_LPM_PER_V[6]*4.0f - VALVE_GAIN_LPM_PER_V[4]*3.0f) + 3.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[7]*4.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[6]*4.0f) / (VALVE_GAIN_LPM_PER_V[8]*5.0f - VALVE_GAIN_LPM_PER_V[6]*4.0f) + 4.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9]*5.0f)) VALVE_FF_VOLTAGE = 5.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[0]*(-1.0f)) && CUR_FLOWRATE < 0.0f) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) / (0.0f - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - 1.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[2]*(-2.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[1]*(-1.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) / ((VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - 2.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[4]*(-3.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[3]*(-2.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) / ((VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - 3.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[6]*(-4.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[5]*(-3.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) / ((VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - 4.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8]*(-5.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[7]*(-4.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) / ((VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) - 5.0f;
+// else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*(-5.0f))) VALVE_FF_VOLTAGE = -5;
+// else VALVE_FF_VOLTAGE = 0;
+// }
+// // VALVE_FF_VOLTAGE = CUR_FLOWRATE * 0.5f;
+//
+// if (CUR_FLOWRATE >= 0) VALVE_FF_VOLTAGE = (float) VELOCITY_COMP_GAIN * 0.001f * VALVE_FF_VOLTAGE * sqrt((float) PRES_SUPPLY - CUR_PRES_A_BAR) * 0.0707f; // 0.0707 = 1/sqrt(200.))
+// else if (CUR_FLOWRATE < 0) VALVE_FF_VOLTAGE = (float) VELOCITY_COMP_GAIN * 0.001f * VALVE_FF_VOLTAGE * sqrt((float) PRES_SUPPLY - CUR_PRES_B_BAR) * 0.0707f;
+//
+// V_out = V_out + VALVE_FF_VOLTAGE;
+// break;
+// }
+
+// case MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION: {
+//
+// pos.err = pos.ref - (float) pos.sen;
+// pos.err_diff = pos.err - pos.err_old;
+// pos.err_old = pos.err;
+// pos.err_sum += pos.err;
+// if (pos.err_sum > 1000) pos.err_sum = 1000;
+// if (pos.err_sum<-1000) pos.err_sum = -1000;
+// VALVE_PWM_RAW_POS = ((float) P_GAIN_JOINT_POSITION * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff) * 0.01f;
+//
+// torq.err = torq.ref - torq.sen;
+// torq.err_diff = torq.err - torq.err_old;
+// torq.err_old = torq.err;
+// torq.err_sum += torq.err;
+// if (torq.err_sum > 1000) torq.err_sum = 1000;
+// if (torq.err_sum<-1000) torq.err_sum = -1000;
+// VALVE_PWM_RAW_TORQ = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
+//
+// valve_pos.ref = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ;
+// VALVE_POS_CONTROL(valve_pos.ref);
+//
+// break;
+// }
+
+// case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING: {
+//
+// break;
+// }
+
+
+// case MODE_TEST_CURRENT_CONTROL: {
+// if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
+// TMR3_COUNT_IREF = 0;
+// }
+// TMR3_COUNT_IREF++;
+//
+// // Set Current Reference
+// float TMR3_CNT_MAX = (float)TMR_FREQ_5k/2.0f;
+// float I_REF_MID = 0.0f;
+// if (TMR3_COUNT_IREF < TMR3_CNT_MAX) {
+// I_REF = I_REF_MID + 1.0f;
+// } else {
+// I_REF = I_REF_MID - 1.0f;
+// }
+//// float T = 1.0; // wave period
+//// I_REF = (5. * sin(2. * 3.1415 * (float) TMR3_COUNT_IREF / (float)TMR_FREQ_5k/ T));
+//// I_REF = (2.0 * sin(2. * 2. * 3.14 * (float) TMR3_COUNT_IREF / 5000.)+(2.0 * sin(2. * 1. * 3.14 * (float)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 5. * 3.14 * (float)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 10. * 3.14 * (float)TMR3_COUNT_IREF/ 5000.)));
+//
+// if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
+// //CAN_TX_PRES((int16_t)(I_REF*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas
+// }
+// break;
+// }
+
+// case MODE_TEST_PWM_CONTROL: {
+// if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
+// TMR3_COUNT_IREF = 0;
+// }
+// TMR3_COUNT_IREF++;
+//
+// // Set PWM reference
+// float TMR3_CNT_MAX = (float)TMR_FREQ_5k/2.0f;
+// //float I_REF_MID = 0.0f;
+// if (TMR3_COUNT_IREF < TMR3_CNT_MAX) {
+// CUR_PWM = 1000;
+// } else {
+// CUR_PWM = -1000;
+// }
+//
+// if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
+// //CAN_TX_PRES((int16_t)(u_CUR[0]*1000.0f), (int16_t) (CUR_CURRENT*1000.0f)); // to check the datas
+// }
+// break;
+// }
+
+
+// case MODE_CURRENT_CONTROL: {
+//
+// cur.ref = cur.ref; // Unit : mA
+// CurrentControl();
+// break;
+// }
+
+// case MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT: {
+// float I_REF_POS_FB = 0.0f; // I_REF by Position Feedback
+// float I_REF_POS_FF = 0.0f; // I_REF by Position Feedforward
+// float I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
+// float I_REF_FORCE_FF = 0.0f; // I_REF by Force Feedforward
+//
+// // feedback input for position control
+// pos.err = pos.ref - pos.sen;
+// float alpha_update_vel = 1.0f/(1.0f+(float)FREQ_TMR4/(2.0f*3.1415f*50.0f)); // f_cutoff : 50Hz
+// float err_diff = (pos.err - pos.err_old)*(float)FREQ_5k;
+// pos.err_diff = (1.0f-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
+// pos.err_old = pos.err;
+// I_REF_POS_FB = 0.001f*((float)P_GAIN_JOINT_POSITION * pos.err + (float)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1f);
+//
+// // feedforward input for position control
+// float Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
+// float K_ff = 1.3f;
+// float K_v = 0.0f;
+// if(Vel_Act_Ref > 0) K_v = 1.0f/100.0f; // open, tuning. (deg/s >> mA)
+// if(Vel_Act_Ref < 0) K_v = 1.0f/100.0f; // close, tuning. (deg/s >> mA)
+// I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
+//
+// // feedback input for position control
+// I_REF_FORCE_FB = 0.0f;
+//
+// // feedforward input for position control
+// I_REF_FORCE_FF = 0.0f;
+//
+// cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
+//
+// CurrentControl();
+//
+// break;
+// }
+
+// case MODE_JOINT_POSITION_PRES_CONTROL_CURRENT: {
+// //float T_REF = 0.0; // Torque Reference
+// float I_REF_FORCE_FB = 0.; // I_REF by Force Feedback
+// float I_REF_VC = 0.; // I_REF for velocity compensation
+//
+// // feedback input for position control
+// //float Joint_Pos_Err = 34.0f-(float) pos.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+// //float Joint_Vel_Err = 0.0f-(float) vel.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+// //float K_spring = 0.7f;
+// //float D_damper = 0.02f;
+//// T_REF = K_spring * pos.err + D_damper * Joint_Vel_Err; // unit : Nm
+//
+// // torque feedback
+// torq.err = torq.ref - torq.sen;
+// // torq.err_diff = torq.err - torq.err_old;
+// // torq.err_old = torq.err;
+// torq.err_sum = torq.err_sum + torq.err/(float)TMR_FREQ_5k;
+// I_REF_FORCE_FB = 0.001f*((float)P_GAIN_JOINT_TORQUE * torq.err + (float)I_GAIN_JOINT_TORQUE * torq.err_sum);
+//
+// // velocity compensation for torque control
+// float Joint_Vel_Act = vel.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+// float K_vc = 1.5f; // Velocity comp. gain
+// float K_v = 0.0f; // Valve gain
+// if(Joint_Vel_Act > 0) K_v = 1.0f/100.0f; // open, tuning
+// if(Joint_Vel_Act < 0) K_v = 1.0f/100.0f; // close, tuning
+// I_REF_VC = K_vc*K_v*Joint_Vel_Act;
+//
+// cur.ref = I_REF_VC + I_REF_FORCE_FB;
+// // cur.ref = I_REF_FORCE_FB;
+//
+// float I_MAX = 10.0f; // Maximum Current : 10mV
+// float Ka = 1.0f/I_GAIN_JOINT_TORQUE;
+// if(cur.ref > I_MAX) {
+// float I_rem = cur.ref-I_MAX;
+// I_rem = Ka*I_rem;
+// cur.ref = I_MAX;
+// torq.err_sum = torq.err_sum - I_rem/(float)TMR_FREQ_5k;
+// } else if(cur.ref < -I_MAX) {
+// float I_rem = cur.ref-(-I_MAX);
+// I_rem = Ka*I_rem;
+// cur.ref = -I_MAX;
+// torq.err_sum = torq.err_sum - I_rem/(float)TMR_FREQ_5k;
+// }
+//
+// CurrentControl();
+//
+//
+// /*
+// float I_REF_POS_FB = 0.0f; // I_REF by Position Feedback
+// float I_REF_POS_FF = 0.0f; // I_REF by Position Feedforward
+// float I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
+// float I_REF_FORCE_FF = 0.0f; // I_REF by Force Feedforward
+//
+// // feedback input for position control
+// pos.err = pos.ref - pos.sen;
+// float alpha_update_vel = 1.0f/(1.0f+(float)FREQ_TMR4/(2.0f*3.1415f*50.0f)); // f_cutoff : 50Hz
+// float err_diff = (pos.err - pos.err_old)*(float)FREQ_5k;
+// pos.err_diff = (1.0f-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
+// pos.err_old = pos.err;
+// I_REF_POS_FB = 0.001f*((float)P_GAIN_JOINT_POSITION * pos.err + (float)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1f);
+//
+// // feedforward input for position control
+// float Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
+// float K_ff = 1.3f;
+// float K_v = 0.0f;
+// if(Vel_Act_Ref > 0) K_v = 1.0f/100.0f; // open, tuning. (deg/s >> mA)
+// if(Vel_Act_Ref < 0) K_v = 1.0f/100.0f; // close, tuning. (deg/s >> mA)
+// I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
+//
+// // feedback input for position control
+// I_REF_FORCE_FB = 0.0f;
+//
+// // feedforward input for position control
+// I_REF_FORCE_FF = 0.0f;
+//
+// cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
+//
+// CurrentControl();
+// */
+//
+// break;
+// }
+
case MODE_TORQUE_SENSOR_NULLING: {
// DAC Voltage reference set
if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
@@ -479,11 +849,61 @@
ROM_RESET_DATA();
+ //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
+
+ //pc.printf("%f\n", TORQUE_VREF);
dac_1 = TORQUE_VREF / 3.3f;
}
TMR3_COUNT_TORQUE_NULL++;
break;
+
+
+
+ // // DAC Voltage reference set
+// if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
+// CUR_PRES_A_sum += pres_A.sen;
+// CUR_PRES_B_sum += pres_B.sen;
+//
+// if (TMR3_COUNT_PRES_NULL % 10 == 0) {
+// CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0f;
+// CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0f;
+// CUR_PRES_A_sum = 0;
+// CUR_PRES_B_sum = 0;
+//
+// float VREF_NullingGain = 0.0003f;
+// PRES_A_VREF = PRES_A_VREF - VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean);
+// PRES_B_VREF = PRES_B_VREF - VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean);
+//
+// if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
+// if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
+// if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f;
+// if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
+//
+// dac_1 = PRES_A_VREF / 3.3f;
+// dac_2 = PRES_B_VREF / 3.3f;
+// }
+// } else {
+// CONTROL_MODE = MODE_NO_ACT;
+// TMR3_COUNT_PRES_NULL = 0;
+// CUR_PRES_A_sum = 0;
+// CUR_PRES_B_sum = 0;
+// CUR_PRES_A_mean = 0;
+// CUR_PRES_B_mean = 0;
+//
+// ROM_RESET_DATA();
+//
+// dac_1 = PRES_A_VREF / 3.3f;
+// dac_2 = PRES_B_VREF / 3.3f;
+// //pc.printf("nulling end");
+// }
+// TMR3_COUNT_PRES_NULL++;
+// break;
+
+
+
+
+
}
// case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: {
@@ -571,6 +991,7 @@
pos.ref_home_pos = pos.sen;
vel.ref_home_pos = 0.0f;
FINDHOME_STAGE = FINDHOME_GOTOLIMIT;
+ CAN_TX_PRES((int16_t)(CONTROL_MODE), (int16_t) (3));
} else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) {
int cnt_check_enc = (TMR_FREQ_5k/500);
if(cnt_findhome%cnt_check_enc == 0) {
@@ -579,6 +1000,7 @@
FINDHOME_POSITION_OLD = FINDHOME_POSITION;
}
cnt_findhome++;
+ //if(cnt_findhome == TMR_FREQ_5k) cnt_findhome = 0;
if (abs(FINDHOME_VELOCITY) <= 1) {
cnt_vel_findhome = cnt_vel_findhome + 1;
@@ -586,14 +1008,14 @@
cnt_vel_findhome = 0;
}
- if ((cnt_vel_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec
+ if ((cnt_vel_findhome < 3*TMR_FREQ_5k) && cnt_findhome <= 10*TMR_FREQ_5k) { // wait for 3sec
//REFERENCE_MODE = MODE_REF_NO_ACT;
if (HOMEPOS_OFFSET > 0) pos.ref_home_pos = pos.ref_home_pos + 2.0f;
else pos.ref_home_pos = pos.ref_home_pos - 2.0f;
pos.err = pos.ref_home_pos - pos.sen;
float VALVE_POS_RAW_POS_FB = 0.0f;
VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f;
- valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER;
+ valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
//float wn_Pos = 2.0f*PI*5.0f; // f_cut : 10Hz Position Control
@@ -623,7 +1045,7 @@
pos.err = pos.ref_home_pos - (float)pos.sen;
float VALVE_POS_RAW_POS_FB = 0.0f;
VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err/(float) ENC_PULSE_PER_POSITION;
- valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER;
+ valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
cnt_findhome++;
@@ -635,7 +1057,7 @@
pos.ref_home_pos = 0.0f;
vel.ref_home_pos = 0.0f;
FINDHOME_STAGE = FINDHOME_INIT;
- CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
+ CONTROL_MODE = MODE_JOINT_CONTROL;
}
}
@@ -739,7 +1161,7 @@
dac_2 = PRES_B_VREF / 3.3f;
}
} else {
- CONTROL_UTILITY_MODE = MODE_NO_ACT;
+ CONTROL_MODE = MODE_NO_ACT;
TMR3_COUNT_PRES_NULL = 0;
CUR_PRES_A_sum = 0;
CUR_PRES_B_sum = 0;
@@ -837,7 +1259,7 @@
}
//ROM_RESET_DATA();
ID_index = 0;
- CONTROL_UTILITY_MODE = MODE_NO_ACT;
+ CONTROL_MODE = MODE_NO_ACT;
}
@@ -984,7 +1406,7 @@
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));
+ DDV_CENTER = 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;
@@ -1069,12 +1491,12 @@
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));
+ DDV_CENTER = 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;
- CONTROL_UTILITY_MODE = MODE_NO_ACT;
+ CONTROL_MODE = MODE_NO_ACT;
DZ_index = 1;
}
}
@@ -1101,7 +1523,7 @@
pos_minus_end = pos.sen;
first_check = 1;
VALVE_FR_timer = 0;
- valve_pos.ref = (float) VALVE_CENTER;
+ valve_pos.ref = DDV_CENTER;
ID_index = 0;
max_check = 0;
min_check = 0;
@@ -1111,12 +1533,12 @@
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]) + (float) VALVE_CENTER;
+ valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_CENTER;
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.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_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) {
@@ -1150,194 +1572,42 @@
ID_index = 0;
first_check = 0;
VALVE_FR_timer = 0;
- CONTROL_UTILITY_MODE = MODE_NO_ACT;
+ CONTROL_MODE = MODE_NO_ACT;
// CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
}
}
break;
}
-
- case MODE_SYSTEM_ID: {
- freq_sysid_Iref = (double) cnt_sysid * DT_TMR3 * 3.;
- valve_pos.ref = 2500.0f * sin(2.0f * 3.14159f * freq_sysid_Iref * (double) cnt_sysid * DT_TMR3);
- CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
- cnt_sysid++;
- if (freq_sysid_Iref >= 300) {
- cnt_sysid = 0;
- CONTROL_UTILITY_MODE = MODE_NO_ACT;
- }
- break;
- }
-
-
-
- default:
- break;
- }
-
- // CONTROL MODE ------------------------------------------------------------
-
- switch (CONTROL_MODE) {
- case MODE_NO_ACT: {
- V_out = 0.0f;
- break;
- }
-
- case MODE_VALVE_POSITION_CONTROL: {
- if ((OPERATING_MODE&0b110) == 0) { //Moog Valve
- I_REF = valve_pos.ref;
- } else if ((OPERATING_MODE&0b110) == 1) { //KNR Valve
- V_out = valve_pos.ref;
- } else { //SW Valve
- VALVE_POS_CONTROL(valve_pos.ref);
- }
-
- break;
- }
-
- case MODE_JOINT_CONTROL: {
-
- float VALVE_POS_RAW_FORCE_FB = 0.0f;
-
-
- double torq_ref = 0.0f;
- //if(TMR3_COUNT_TEST % (int) (50) == 0){
- pos.err = pos.ref - pos.sen; //[pulse]
- vel.err = vel.ref - vel.sen; //[pulse/s]
- torq_ref = torq.ref + (K_SPRING * pos.err * 0.01f + D_DAMPER * vel.err * 0.0001f) / ENC_PULSE_PER_POSITION; //[N]
- //torq_ref_logging = torq_ref
- //}
-
- // torque feedback
- torq.err = torq_ref - torq.sen; //[pulse]
- torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse]
-// if (torq.err_sum > 1000) torq.err_sum = 1000;
-// if (torq.err_sum<-1000) torq.err_sum = -1000;
-
-
- VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) /(float) TORQUE_SENSOR_PULSE_PER_TORQUE * 0.01f
- + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err /(float) ENC_PULSE_PER_POSITION + 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;
- }
-
- if(I_GAIN_JOINT_TORQUE != 0){
- double Ka = 1.0f / (double) I_GAIN_JOINT_TORQUE * (float) TORQUE_SENSOR_PULSE_PER_TORQUE * 100.0f;
- if(valve_pos.ref>VALVE_MAX_POS){
- double valve_pos_rem = valve_pos.ref - VALVE_MAX_POS;
- valve_pos_rem = valve_pos_rem * Ka;
- valve_pos.ref = VALVE_MAX_POS;
- torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k;
- }
- else if(valve_pos.ref < VALVE_MIN_POS){
- double valve_pos_rem = valve_pos.ref - VALVE_MIN_POS;
- valve_pos_rem = valve_pos_rem * Ka;
- valve_pos.ref = VALVE_MIN_POS;
- torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k;
- }
- }
-
- VALVE_POS_CONTROL(valve_pos.ref);
-
- //TMR3_COUNT_TEST++;
-
- break;
- }
-
- case MODE_VALVE_OPEN_LOOP: {
- V_out = (float) Vout.ref;
- break;
- }
default:
break;
}
-
- if ((OPERATING_MODE&0b110) == 0 || (OPERATING_MODE&0b110) == 1) { //Moog Valve or KNR Valve
-
- ////////////////////////////////////////////////////////////////////////////
- //////////////////////////// CURRENT CONTROL //////////////////////////////
- ////////////////////////////////////////////////////////////////////////////
- if (CURRENT_CONTROL_MODE) {
- double alpha_update_Iref = 1.0f / (1.0f + TMR_FREQ_5k / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz
- I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF;
-
- I_ERR = I_REF_fil - CUR_CURRENT_mA;
- I_ERR_INT = I_ERR_INT + (I_ERR) * DT_TMR3;
-
-
- // Moog Valve Current Control Gain
- double R_model = 539.0f; // ohm
- double L_model = 1.2f;
- double w0 = 2.0f * 3.14f * 150.0f;
- double KP_I = 0.1f * L_model*w0;
- double KI_I = 0.1f * R_model*w0;
-
- // KNR Valve Current Control Gain
- if ((OPERATING_MODE & 0b110) == 1) { // KNR Valve
- R_model = 163.0f; // ohm
- L_model = 1.0f;
- w0 = 2.0f * 3.14f * 80.0f;
- KP_I = 1.0f * L_model*w0;
- KI_I = 0.08f * R_model*w0;
- }
-
- double FF_gain = 1.0f;
-
- VALVE_PWM_RAW = KP_I * I_ERR + KI_I * I_ERR_INT;
- // VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model*I_REF); // Unit : mV
- I_REF_fil_diff = I_REF_fil - I_REF_fil_old;
- I_REF_fil_old = I_REF_fil;
- VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil + L_model * I_REF_fil_diff * TMR_FREQ_5k); // Unit : mV
- double V_MAX = 12000.; // Maximum Voltage : 12V = 12000mV
-
- double Ka = 3.0f / KP_I;
- if (VALVE_PWM_RAW > V_MAX) {
- V_rem = VALVE_PWM_RAW - V_MAX;
- V_rem = Ka*V_rem;
- VALVE_PWM_RAW = V_MAX;
- I_ERR_INT = I_ERR_INT - V_rem * DT_TMR3;
- } else if (VALVE_PWM_RAW < -V_MAX) {
- V_rem = VALVE_PWM_RAW - (-V_MAX);
- V_rem = Ka*V_rem;
- VALVE_PWM_RAW = -V_MAX;
- I_ERR_INT = I_ERR_INT - V_rem * DT_TMR3;
- }
- Cur_Valve_Open_pulse = CUR_CURRENT_mA / mA_PER_pulse;
- } else {
- VALVE_PWM_RAW = I_REF * mV_PER_mA;
- Cur_Valve_Open_pulse = I_REF / mA_PER_pulse;
- }
-
- ////////////////////////////////////////////////////////////////////////////
- ///////////////// Dead Zone Cancellation & Linearization //////////////////
- ////////////////////////////////////////////////////////////////////////////
- // Dead Zone Cancellation (Mechanical Valve dead-zone)
- if (FLAG_VALVE_DEADZONE) {
- if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV
- else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV
-
- VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV
-
- } else {
- VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW;
- }
-
- // Output Voltage Linearization
- double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ; // Unit : mV
- double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin);
-
- // Dead Zone Cancellation (Electrical dead-zone)
- if (CUR_PWM_lin > 0) V_out = (int) CUR_PWM_lin + 140;
- else if (CUR_PWM_lin < 0) V_out = (int) CUR_PWM_lin - 140;
- else V_out = CUR_PWM_lin;
- }
-
-
+// if (FLAG_VALVE_OUTPUT_CALIB) {
+// // Valve Dead Zone (Mechanical dead-zone canceling)
+// // if (CONTROL_MODE != MODE_VALVE_NULLING_AND_DEADZONE_SETTING) {
+// // if (V_out > 0.) {
+// // VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_DEADZONE_PLUS;
+// // } else if (V_out < 0.) {
+// // VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_DEADZONE_MINUS;
+// // } else VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_CENTER;
+// // } else VALVE_PWM_VALVE_DZ = (int) V_out;
+//
+// if(V_out>0) V_out = V_out + VALVE_DEADZONE_PLUS;
+// else if(V_out<0) V_out = V_out + VALVE_DEADZONE_MINUS;
+//
+// VALVE_PWM_VALVE_DZ = V_out + VALVE_CENTER;
+//
+// // Output Voltage Linearization and Dead Zone Cancellation (Electrical dead-zone)
+// float CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ/5.0f*1000.0f; // convert PWM duty to mV
+// float CUR_PWM_DZ = PWM_duty_byLT(CUR_PWM_nonlin);
+//
+// if (CUR_PWM_DZ > 0) V_out = (int)CUR_PWM_DZ + 143;
+// else if (CUR_PWM_DZ < 0) V_out = (int)CUR_PWM_DZ - 138;
+// else V_out = CUR_PWM_DZ;
+// } else {
+// V_out = V_out;
+// }
/*******************************************************
*** PWM
@@ -1349,6 +1619,10 @@
}
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
+// else if (PWM_out < -0.41667) PWM_out=-0.41667;
+
// Saturation of output voltage to 12.0V
if(PWM_out > 1.0f) PWM_out=1.0f;
else if (PWM_out < -1.0f) PWM_out=-1.0f;
@@ -1366,17 +1640,19 @@
TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
- if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
+
+ if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/canfreq) == 0) {
+// if (TMR2_COUNT_CAN_TX % 10 == 0) {
// Position, Velocity, and Torque (ID:1200)
- if (flag_data_request[0] == LOW) {
- if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
+ if (flag_data_request[0] == HIGH) {
+ if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
if (SENSING_MODE == 0) {
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), (int16_t) (vel.sen/10.0f), (int16_t) (pres_A.sen*10.0f), (int16_t) (pres_B.sen*10.0f));
}
- } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
+ } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
if (SENSING_MODE == 0) {
CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f));
} else if (SENSING_MODE == 1) {
@@ -1384,38 +1660,30 @@
}
}
}
- if (flag_data_request[1] == LOW) {
+ if (flag_data_request[1] == HIGH) {
//valve position
double t_value = 0;
- if(value>=(float) VALVE_CENTER) {
- t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+ if(value>=DDV_CENTER) {
+ t_value = 10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
} else {
- t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+ t_value = -10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
}
- CAN_TX_TORQUE((int16_t) (t_value)); //1300
- }
-
-
- if (flag_data_request[2] == LOW) {
- //pressure A and B
- CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar) //1400
+ CAN_TX_TORQUE((int16_t) (t_value));
}
-
- //If it doesn't rest, below can can not work.
- for (can_rest = 0; can_rest < 10000; can_rest++) {
- ;
+
+ if (flag_data_request[2] == HIGH) {
+ //pressure A and B
+ CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (MODE_POS_FT_TRANS * 100.0f)); // CUR_PRES_X : 0(0bar)~4096(210bar)
}
-
- if (flag_data_request[3] == LOW) {
+
+ if (flag_data_request[3] == HIGH) {
//PWM
- CAN_TX_PWM((int16_t) cur.sen); //1500
+ CAN_TX_PWM((int16_t) VALVE_DEADZONE_PLUS);
}
- //for (i = 0; i < 10000; i++) {
-// ;
-// }
- if (flag_data_request[4] == LOW) {
+
+ if (flag_data_request[4] == HIGH) {
//valve position
- CAN_TX_VALVE_POSITION((int16_t) (CAN_FREQ), (int16_t) (D_DAMPER), (int16_t) OPERATING_MODE); //1600
+ CAN_TX_VALVE_POSITION((int16_t) (K_SPRING), (int16_t) (D_DAMPER), (int16_t) VALVE_POS_RAW_FORCE_FB_LOGGING);
}
// Others : Reference position, Reference FT, PWM, Current (ID:1300)
@@ -1430,6 +1698,8 @@
}
TMR2_COUNT_CAN_TX++;
+
+
}
TIM3->SR = 0x0; // reset the status register
@@ -1440,63 +1710,58 @@
//unsigned long CNT_TMR5 = 0;
//float FREQ_TMR5 = (float)FREQ_500;
//float DT_TMR5 = (float)DT_500;
-
//extern "C" void TIM2_IRQHandler(void)
//{
// LED = 1;
// if (TIM2->SR & TIM_SR_UIF ) {
//
// //CAN ----------------------------------------------------------------------
-// //if (flag_data_request[0] == LOW) {
+// if (flag_data_request[0] == HIGH) {
// //position+velocity
// CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f));
-// //}
+// }
//
-// //if (flag_data_request[1] == LOW) {
+// if (flag_data_request[1] == HIGH) {
// //valve position
// double t_value = 0;
-// if(value>=(double)VALVE_CENTER) {
-// t_value = 10000.0f*((double)value-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
+// if(value>=DDV_CENTER) {
+// t_value = 10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
// } else {
-// t_value = -10000.0f*((double)value-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
+// t_value = -10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
// }
// CAN_TX_TORQUE((int16_t) (t_value));
-// //}
+// }
//
-// //if (flag_data_request[2] == LOW) {
+// if (flag_data_request[2] == HIGH) {
// //pressure A and B
// CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (MODE_POS_FT_TRANS * 100.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.);
+// // CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);
//
-// //}
+// }
//
-// //if (flag_data_request[3] == LOW) {
+// if (flag_data_request[3] == HIGH) {
// //PWM
-// int i = 0;
-// for (i = 0; i < 10000; i++) {
-// ;
-// }
// CAN_TX_PWM((int16_t) VALVE_DEADZONE_PLUS);
// // CAN_TX_PWM((int16_t) cnt_vel_findhome);
// // CAN_TX_PWM((int16_t) (VALVE_VOLTAGE * 1000.));
// // CAN_TX_PWM((int16_t) (VALVE_VOLTAGE_VALVE_DZ * 1000.));
//
-// //}
-//
-// //if (flag_data_request[4] == LOW) {
-// //valve position
+// }
//
-// for (i = 0; i < 10000; i++) {
-// ;
-// }
+// if (flag_data_request[4] == HIGH) {
+// //valve position
// CAN_TX_VALVE_POSITION((int16_t) (K_SPRING), (int16_t) (D_DAMPER), (int16_t) VALVE_POS_RAW_FORCE_FB_LOGGING);
-// //CAN_TX_VALVE_POSITION((int16_t) ((float) VALVE_CENTER * 10.0f), (int16_t) valve_pos.ref, (int16_t) V_out);
+// //CAN_TX_VALVE_POSITION((int16_t) (DDV_CENTER * 10.0f), (int16_t) valve_pos.ref, (int16_t) V_out);
//
// //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
// // 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);
-// //}
+// }
//
//
// }
@@ -1504,39 +1769,39 @@
//}
-//
-//void CurrentControl()
-//{
-// cur.err = cur.ref - cur.sen;
-// cur.err_int = cur.err_int + cur.err*DT_TMR4;
-// cur.err_diff = (cur.err - cur.err_old)*FREQ_TMR4;
-// cur.err_old = cur.err;
-//
-// float R_model = 150.0f; // ohm
-// float L_model = 0.3f;
-// float w0 = 2.0f*3.14f*90.0f;
-// float KP_I = L_model*w0;
-// float KI_I = R_model*w0;
-// float KD_I = 0.0f;
-//
-// float FF_gain = 0.0f;
-// V_out = (int) (KP_I * cur.err + KI_I * cur.err_int + KD_I * cur.err_diff);
-// // V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV
-// V_out = V_out + FF_gain * (R_model*cur.ref + L_model*cur.ref_diff); // Unit : mV
-//
-// float Ka = 5.0f/KP_I;
-// if(V_out > V_MAX) {
-// V_rem = V_out-V_MAX;
-// V_rem = Ka*V_rem;
-// V_out = V_MAX;
-// cur.err_int = cur.err_int - V_rem*DT_5k;
-// } else if(V_out < -V_MAX) {
-// V_rem = V_out-(-V_MAX);
-// V_rem = Ka*V_rem;
-// V_out = -V_MAX;
-// cur.err_int = cur.err_int - V_rem*DT_5k;
-// }
-//}
+
+void CurrentControl()
+{
+ cur.err = cur.ref - cur.sen;
+ cur.err_int = cur.err_int + cur.err*DT_TMR4;
+ cur.err_diff = (cur.err - cur.err_old)*FREQ_TMR4;
+ cur.err_old = cur.err;
+
+ float R_model = 150.0f; // ohm
+ float L_model = 0.3f;
+ float w0 = 2.0f*3.14f*90.0f;
+ float KP_I = L_model*w0;
+ float KI_I = R_model*w0;
+ float KD_I = 0.0f;
+
+ float FF_gain = 0.0f;
+ V_out = (int) (KP_I * cur.err + KI_I * cur.err_int + KD_I * cur.err_diff);
+ // V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV
+ V_out = V_out + FF_gain * (R_model*cur.ref + L_model*cur.ref_diff); // Unit : mV
+
+ float Ka = 5.0f/KP_I;
+ if(V_out > V_MAX) {
+ V_rem = V_out-V_MAX;
+ V_rem = Ka*V_rem;
+ V_out = V_MAX;
+ cur.err_int = cur.err_int - V_rem*DT_5k;
+ } else if(V_out < -V_MAX) {
+ V_rem = V_out-(-V_MAX);
+ V_rem = Ka*V_rem;
+ V_out = -V_MAX;
+ cur.err_int = cur.err_int - V_rem*DT_5k;
+ }
+}
@@ -1545,3 +1810,4 @@
+
--- a/setting.h Wed Feb 26 12:51:52 2020 +0000 +++ b/setting.h Tue Mar 03 12:27:53 2020 +0000 @@ -110,11 +110,13 @@ extern uint8_t CONTROL_MODE; extern uint8_t OPERATING_MODE; extern uint8_t SENSING_MODE; -extern uint8_t CONTROL_UTILITY_MODE; extern uint8_t CURRENT_CONTROL_MODE; extern uint8_t FLAG_VALVE_DEADZONE; +extern uint8_t SETTING_SWITCH; +extern uint8_t SETTING_SWITCH_OLD; extern uint8_t REFERENCE_MODE; extern int16_t CAN_FREQ; +extern int CAN_FREQUENCY; extern int16_t DIR_JOINT_ENC; extern int16_t DIR_VALVE; extern int16_t DIR_VALVE_ENC; @@ -176,8 +178,8 @@ extern int16_t STROKE; -//extern int16_t VALVE_LIMIT_PLUS; -//extern int16_t VALVE_LIMIT_MINUS; +extern int16_t VALVE_LIMIT_PLUS; +extern int16_t VALVE_LIMIT_MINUS; extern float ENC_PULSE_PER_POSITION; extern float TORQUE_SENSOR_PULSE_PER_TORQUE; @@ -219,6 +221,7 @@ extern float CUR_TORQUE; extern float CUR_PRES_A; extern float CUR_PRES_B; +extern int CUR_PWM; extern int CUR_VALVE_POSITION; extern unsigned int TMR2_COUNT_LED1; @@ -237,20 +240,25 @@ extern int MODE_POS_FT_TRANS; -extern float CUR_CURRENT_mA; +//////////////////////////////////////////////////////////////////////////////// +////////////////////////////// SEUNGHOON ADD /////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// + extern float CUR_PRES_A_BAR; extern float CUR_PRES_B_BAR; extern float CUR_TORQUE_NM; extern float CUR_TORQUE_NM_PRESS; extern float PRES_A_VREF; +extern float PRES_A_VREF_TEST; extern float PRES_B_VREF; +extern float PRES_B_VREF_TEST; extern float TORQUE_VREF; extern float VALVE_PWM_RAW_FB; extern float VALVE_PWM_RAW_FF; -extern float VALVE_PWM_RAW; extern int VALVE_PWM_VALVE_DZ; +extern int VALVE_INPUT_PWM; extern float VALVE_GAIN_LPM_PER_V[10]; extern float VALVE_POS_VS_PWM[25]; @@ -258,7 +266,9 @@ extern int VALVE_MAX_POS; extern int VALVE_MIN_POS; +extern float DDV_CENTER; extern int VALVE_POS_NUM; +extern int VALVE_CENTER_OFFSET_times10; extern float VALVE_CENTER_OFFSET; extern float VALVE_DZ_MINUS_OFFSET; extern float VALVE_DZ_PLUS_OFFSET; @@ -275,6 +285,13 @@ extern int TMR3_COUNT_JOINT; extern int TMR3_COUNT_ROTARY_FRIC_TUNE; +extern bool FLAG_REFERENCE_VALVE_PWM; +extern bool FLAG_REFERENCE_VALVE_POSITION; +extern bool FLAG_REFERENCE_JOINT_POSITION; +extern bool FLAG_REFERENCE_JOINT_TORQUE; +extern bool FLAG_REFERENCE_PRES_DIFF; +extern bool FLAG_REFERENCE_CURRENT; + extern float TUNING_TIME; extern float REFERENCE_FREQ; @@ -354,8 +371,6 @@ extern int fl_temp_cnt2; extern int cur_vel_sum; -extern float Cur_Valve_Open_pulse; - // find home extern int CUR_VELOCITY_OLD; extern int cnt_findhome; @@ -374,20 +389,11 @@ extern float freq_fric_tune; +extern bool FLAG_VALVE_OUTPUT_CALIB; + extern uint32_t TMR3_COUNT_CAN_TX; -// Current Control Variables -extern double I_REF; -extern double I_REF_fil; -extern double I_ERR; -extern double I_ERR_INT; -extern double I_REF_fil_old; -extern double I_REF_fil_diff; - -// system id -extern int cnt_sysid; -extern double freq_sysid_Iref; - +extern float I_REF; extern int TMR3_COUNT_IREF; extern float CUR_CURRENT; extern float u_CUR[3]; @@ -399,20 +405,7 @@ extern float alpha_trans; - -extern float V_out; -extern float V_rem; -extern float V_MAX; - -extern float PWM_out; - -extern double K_v; -extern double mV_PER_mA; -extern double mV_PER_pulse; -extern double mA_PER_pulse; - -extern int timer_while; -extern int while_index; +//extern int h1, h2, h3, h4, h5, h6; @@ -430,4 +423,3 @@ -