Buyoun Cho
[Ver 1.0] The code was given by Seunghoon shin, used for hydraulic quadrupedal robot. Buyoun Cho will revise the code for Post-LIGHT (the robot name is not determined yet).
Revision 230:2c3e5ecbe7e1, committed 2021-04-21
- Comitter:
- jobuuu
- Date:
- Wed Apr 21 04:20:39 2021 +0000
- Parent:
- 229:7a1c46b9b471
- Commit message:
- Synchronize to HCB_210420 (MZ-board)
Changed in this revision
--- a/CAN/function_CAN.cpp Tue Apr 20 00:42:45 2021 +0000
+++ b/CAN/function_CAN.cpp Wed Apr 21 04:20:39 2021 +0000
@@ -14,10 +14,11 @@
int CID_TX_INFO = 1100;
int CID_TX_POS_VEL_TORQ = 1200;
int CID_TX_PWM = 1300;
+int CID_TX_SOMETHING = 1400;
+
int CID_TX_CURRENT = 1400;
int CID_TX_VOUT = 1500;
int CID_TX_VALVE_POSITION = 1600;
-int CID_TX_SOMETHING = 1700;
// variables
uint8_t can_index = 0;
@@ -32,6 +33,31 @@
extern float input_NN[];
/*******************************************************************************
+ * State Class functions
+ ******************************************************************************/
+void State::UpdateSen(float sen_new, float Freq_update, float f_cut) {
+ if(f_cut<=0.0f) f_cut=0.001f;
+
+ this->sen_diff = (sen_new-this->sen)*Freq_update;
+ float alpha_update = 1.0f / (1.0f + Freq_update / (2.0f * 3.14f * f_cut)); // f_cutoff : 100Hz
+ this->sen = (1.0f - alpha_update) * this->sen + alpha_update * sen_new;
+}
+
+void State::UpdateRef(float ref_new) {
+ this->ref = ref_new;
+}
+
+void State::Reset() {
+ this->sen = 0.0f;
+ this->sen_diff = 0.0f;
+ this->ref = 0.0f;
+ this->err = 0.0f;
+ this->err_int = 0.0f;
+ this->err_old = 0.0f;
+ this->err_diff = 0.0f;
+}
+
+/*******************************************************************************
* CAN functions
******************************************************************************/
void CAN_ID_INIT(void)
@@ -232,6 +258,18 @@
CONTROL_UTILITY_MODE = 22;
break;
}
+
+ case CRX_ASK_VARIABLE_SUPPLY:
+ {
+ CAN_TX_VARIABLE_SUPPLY_ONOFF();
+ break;
+ }
+
+ case CRX_SET_VARIABLE_SUPPLY:
+ {
+ SUPPLY_PRESSURE_UPDATE = msg.data[1];
+ break;
+ }
case CRX_ASK_PID_GAIN: {
CAN_TX_PID_GAIN(msg.data[1]);
@@ -392,22 +430,16 @@
break;
}
- case CRX_ASK_PRES: {
- CAN_TX_PRES_A_AND_B();
-// SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0);
- //dac_1 = PRES_A_VREF;
- //dac_2 = PRES_B_VREF;
-
+ case CRX_ASK_SUP_PRES: {
+ CAN_TX_SUP_PRES();
break;
}
- case CRX_SET_PRES: {
- PRES_SUPPLY = (int16_t) (msg.data[1] | msg.data[2] << 8);
- PRES_RETURN = (int16_t) (msg.data[3] | msg.data[4] << 8);
- spi_eeprom_write(RID_PRES_SUPPLY, (int16_t) PRES_SUPPLY);
- spi_eeprom_write(RID_PRES_RETURN, (int16_t) PRES_RETURN);
-
-
+ case CRX_SET_SUP_PRES: {
+ int16_t temp = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ spi_eeprom_write(RID_PRES_SUPPLY, temp);
+ PRES_SUPPLY_NOM = (float)temp;
+ PRES_SUPPLY = PRES_SUPPLY_NOM;
break;
}
@@ -634,65 +666,58 @@
} else if(address==CID_RX_REF_POSITION) {
- int16_t temp_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
+ int16_t temp_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
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
- REF_POSITION = (double)temp_pos * 10.0f;
- REF_VELOCITY = (double)temp_vel * 256.0f;
+ REF_POSITION = (float)temp_pos * 1.0f / ENC_PULSE_PER_POSITION; // pulse >> deg
+ REF_VELOCITY = (float)temp_vel * 10.0f / ENC_PULSE_PER_POSITION; // pulse/s >> deg/s
+ REF_TORQUE = (float)temp_torq * 0.01f / TORQUE_SENSOR_PULSE_PER_TORQUE; // pulse >> Nm
} else { //Linear Actuator
- REF_POSITION = (double)temp_pos * 10.0f;
- REF_VELOCITY = (double)temp_vel * 256.0f;
+ REF_POSITION = (float)temp_pos * 4.0f / ENC_PULSE_PER_POSITION; // pulse >> mm
+ REF_VELOCITY = (float)temp_vel * 100.0f / ENC_PULSE_PER_POSITION; // pulse/s >> mm/s
+ REF_FORCE = (float)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE; // pulse >> N
}
- 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(SUPPLY_PRESSURE_UPDATE == 1) {
+ int16_t temp_REF_Ps = (int16_t) (msg.data[6] | msg.data[7] << 8);
+ PRES_SUPPLY = ((float)temp_REF_Ps) / 100.0;
+ if(PRES_SUPPLY<35.0f) PRES_SUPPLY = 35.0f;
+ else if(PRES_SUPPLY>210.0f) PRES_SUPPLY = 210.0f;
+ } else {
+ PRES_SUPPLY = PRES_SUPPLY_NOM;
+ }
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), (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)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
- }
+ CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.1f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.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)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
- }
+ CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION*0.25f), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.01f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
}
}
- // PWM, ID:1300
+ // Valve Position (ID:1300)
if (flag_data_request[1] == HIGH) {
- CAN_TX_PWM((int16_t) (V_out)); //1300
+ CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
}
- // Reference Current, Current Current (ID:1400)
+ // Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400)
if (flag_data_request[2] == HIGH) {
- CAN_TX_CURRENT((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse)); // 1400
+ CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (0), (int16_t) (0));
}
-
- // Reference Current, Current Current (ID:1700)
- if (flag_data_request[3] == HIGH) {
- CAN_TX_SOMETHING((int16_t) (0), (int16_t) (0), (int16_t) (0), (int16_t) (0)); // 1700
- }
-
}
-
} 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
- valve_pos.ref = (double) temp_ref_valve_pos;
+ valve_pos.ref = (double) temp_ref_valve_pos; // Unit : pulse (0~10000)
} else if (((OPERATING_MODE&0b110)>>1) == 1) { //KNR Valve
- valve_pos.ref = (double) temp_ref_valve_pos;
+ valve_pos.ref = (double) temp_ref_valve_pos; // Unit : pulse (0~30000)
} 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;
@@ -851,6 +876,16 @@
can.write(temp_msg);
}
+void CAN_TX_VARIABLE_SUPPLY_ONOFF(void)
+{
+ CANMessage temp_msg;
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 2;
+ temp_msg.data[0] = (uint8_t) CTX_SEND_VARIABLE_SUPPLY;
+ temp_msg.data[1] = (uint8_t) SUPPLY_PRESSURE_UPDATE;
+
+ can.write(temp_msg);
+}
void CAN_TX_PID_GAIN(int t_type)
{
@@ -992,17 +1027,18 @@
can.write(temp_msg);
}
-void CAN_TX_PRES_A_AND_B(void)
+void CAN_TX_SUP_PRES(void)
{
CANMessage temp_msg;
+ int16_t temp_PRES_SUPPLY = (int16_t) (PRES_SUPPLY);
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
- temp_msg.data[0] = (uint8_t) CTX_SEND_PRES;
- temp_msg.data[1] = (uint8_t) PRES_SUPPLY;
- temp_msg.data[2] = (uint8_t) (PRES_SUPPLY >> 8);
- temp_msg.data[3] = (uint8_t) PRES_RETURN;
- temp_msg.data[4] = (uint8_t) (PRES_RETURN >> 8);
+ temp_msg.data[0] = (uint8_t) CTX_SEND_SUP_PRES;
+ temp_msg.data[1] = (uint8_t) temp_PRES_SUPPLY;
+ temp_msg.data[2] = (uint8_t) (temp_PRES_SUPPLY >> 8);
+ temp_msg.data[3] = 0;
+ temp_msg.data[4] = 0;
can.write(temp_msg);
}
--- a/CAN/function_CAN.h Tue Apr 20 00:42:45 2021 +0000
+++ b/CAN/function_CAN.h Wed Apr 21 04:20:39 2021 +0000
@@ -45,7 +45,10 @@
#define CRX_SET_VOLTAGE_SUPPLY 112
#define CRX_ASK_VOLTAGE_VALVE 13
#define CRX_SET_VOLTAGE_VALVE 113
-#define CRX_SET_HOMEPOS 114
+#define CRX_SET_HOMEPOS 114
+#define CRX_ASK_VARIABLE_SUPPLY 15
+#define CRX_SET_VARIABLE_SUPPLY 115
+
#define CRX_ASK_PID_GAIN 20
#define CRX_SET_PID_GAIN 120
#define CRX_ASK_VALVE_DEADZONE 21
@@ -62,8 +65,8 @@
#define CRX_SET_CHAMBER_VOLUME 127
#define CRX_ASK_PISTON_AREA 28
#define CRX_SET_PISTON_AREA 128
-#define CRX_ASK_PRES 29
-#define CRX_SET_PRES 129
+#define CRX_ASK_SUP_PRES 29
+#define CRX_SET_SUP_PRES 129
#define CRX_ASK_ENC_LIMIT 30
#define CRX_SET_ENC_LIMIT 130
#define CRX_ASK_STROKE 31
@@ -119,6 +122,7 @@
#define CTX_SEND_VALVE_ENC_DIR 11
#define CTX_SEND_VOLTAGE_SUPPLY 12
#define CTX_SEND_VOLTAGE_VALVE 13
+#define CTX_SEND_VARIABLE_SUPPLY 15
#define CTX_SEND_PID_GAIN 20
#define CTX_SEND_VALVE_DEADZONE 21
#define CTX_SEND_VELOCITY_COMP_GAIN 22
@@ -127,7 +131,7 @@
#define CTX_SEND_BULK_MODULUS 26
#define CTX_SEND_CHAMBER_VOLUME 27
#define CTX_SEND_PISTON_AREA 28
-#define CTX_SEND_PRES 29
+#define CTX_SEND_SUP_PRES 29
#define CTX_SEND_ENC_LIMIT 30
#define CTX_SEND_STROKE 31
#define CTX_SEND_VALVE_LIMIT 32
@@ -168,6 +172,8 @@
void CAN_TX_VALVE_ENC_DIR(void);
void CAN_TX_VOLTAGE_SUPPLY(void);
void CAN_TX_VOLTAGE_VALVE(void);
+void CAN_TX_VARIABLE_SUPPLY_ONOFF(void);
+
void CAN_TX_PID_GAIN(int t_type);
void CAN_TX_VALVE_DEADZONE(void);
void CAN_TX_VELOCITY_COMP_GAIN(void);
@@ -176,7 +182,7 @@
void CAN_TX_BULK_MODULUS(void);
void CAN_TX_CHAMBER_VOLUME(void);
void CAN_TX_PISTON_AREA(void);
-void CAN_TX_PRES_A_AND_B(void);
+void CAN_TX_SUP_PRES(void);
void CAN_TX_ENC_LIMIT(void);
void CAN_TX_STROKE(void);
void CAN_TX_VALVE_LIMIT(void);
@@ -199,10 +205,8 @@
{
public:
float sen;
+ float sen_diff;
float ref;
- float ref_old;
- float ref_diff;
- float ref_home_pos;
float err;
float err_int;
float err_old;
@@ -211,20 +215,23 @@
public:
State(){
sen = 0.0f;
+ sen_diff = 0.0f;
ref = 0.0f;
- ref_old = 0.0f;
- ref_diff = 0.0f;
- ref_home_pos = 0.0f;
err = 0.0f;
err_int = 0.0f;
err_old = 0.0f;
err_diff = 0.0f;
}
+
+ void UpdateSen(float sen_new, float Freq_update, float f_cut = 1000.0f);
+ void UpdateRef(float ref_new);
+ void Reset();
};
extern State pos;
extern State vel;
extern State Vout;
+extern State force;
extern State torq;
extern State torq_dot;
extern State pres_A;
--- a/SPI_EEP_ENC/SPI_EEP_ENC.h Tue Apr 20 00:42:45 2021 +0000 +++ b/SPI_EEP_ENC/SPI_EEP_ENC.h Wed Apr 21 04:20:39 2021 +0000 @@ -80,7 +80,7 @@ #define RID_HOMEPOS_OFFSET 41 #define RID_HOMEPOS_VALVE_OPENING 42 -#define RID_TORQUE_SENSOR_VREF 45 +#define RID_FORCE_SENSOR_VREF 45 #define RID_PRES_A_SENSOR_VREF 50 #define RID_PRES_B_SENSOR_VREF 51
--- a/function_utilities/function_utilities.cpp Tue Apr 20 00:42:45 2021 +0000
+++ b/function_utilities/function_utilities.cpp Wed Apr 21 04:20:39 2021 +0000
@@ -14,6 +14,8 @@
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 SENSING_MODE = 0; // (0 : torque, 1: pressure)
+uint8_t SUPPLY_PRESSURE_UPDATE = 0; // (0 : Update Off (constant Ps) , 1 : Update On (variable Ps))
+
uint8_t CONTROL_UTILITY_MODE = 0;
uint8_t CURRENT_CONTROL_MODE = 0; // (0 : pwm, 1 : current control)
uint8_t FLAG_VALVE_DEADZONE = 0;
@@ -74,9 +76,8 @@
float PISTON_AREA_alpha;
float alpha3 = 1.0f;
-
-int16_t PRES_SUPPLY;
-int16_t PRES_RETURN;
+float PRES_SUPPLY_NOM = 70.0f;
+float PRES_SUPPLY = 70.0f;
int16_t ENC_LIMIT_PLUS;
int16_t ENC_LIMIT_MINUS;
@@ -96,8 +97,8 @@
//int16_t VALVE_LIMIT_PLUS;
//int16_t VALVE_LIMIT_MINUS;
-float ENC_PULSE_PER_POSITION;
-float TORQUE_SENSOR_PULSE_PER_TORQUE;
+float ENC_PULSE_PER_POSITION = 1.0f;
+float TORQUE_SENSOR_PULSE_PER_TORQUE = 1.0f;
float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
float PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
@@ -113,11 +114,13 @@
float DAC_REF;
float DAC_RESOL;
-float REF_POSITION;
-float REF_VELOCITY;
-float REF_TORQUE;
-float REF_TORQUE_OLD;
-float REF_PRES_DIFF;
+float REF_FORCE = 0.0;
+float REF_TORQUE = 0.0;
+float REF_POSITION = 0.0;
+float REF_VELOCITY = 0.0;
+
+float REF_POSITION_FINDHOME = 0.0;
+
int16_t REF_PWM;
int16_t REF_VALVE_POSITION;
int16_t REF_CURRENT;
@@ -125,19 +128,11 @@
int REF_MOVE_TIME_5k;
int INIT_REF_PWM;
int INIT_REF_VALVE_POS;
-int32_t INIT_REF_POS;
int INIT_REF_VEL;
int INIT_REF_TORQUE;
int INIT_REF_PRES_DIFF;
int INIT_REF_CURRENT;
-int CUR_POSITION;
-int CUR_VELOCITY;
-float CUR_TORQUE;
-float CUR_PRES_A;
-float CUR_PRES_B;
-int CUR_VALVE_POSITION;
-
unsigned int TMR2_COUNT_LED1;
unsigned int TMR2_COUNT_LED2;
unsigned int TMR2_COUNT_CAN_TX = 0;
@@ -158,14 +153,12 @@
int cnt_buffer = 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;
+float FORCE_VREF = 0.0f;
float PRES_A_VREF = 0.0f;
float PRES_B_VREF = 0.0f;
-float TORQUE_VREF = 0.0f;
float VALVE_PWM_RAW_FB = 0.0f;
float VALVE_PWM_RAW_FF = 0.0f;
@@ -209,16 +202,10 @@
JUMP_LANDING, //4
};
-float CUR_PRES_DIFF_BAR = 0.0f;
-float CUR_PRES_A_sum = 0.0f;
-float CUR_PRES_B_sum = 0.0f;
-float CUR_PRES_A_mean = 0.0f;
-float CUR_PRES_B_mean = 0.0f;
-float CUR_TORQUE_sum = 0.0f;
-float CUR_TORQUE_mean = 0.0f;
-float PRES_A_NULL = 300.0f;
-float PRES_B_NULL = 300.0f;
-float TORQUE_NULL = 3900.0f;
+
+float PRES_A_NULL_pulse = 300.0f;
+float PRES_B_NULL_pulse = 300.0f;
+float FORCE_NULL_pulse = 3900.0f;
float Ref_Valve_Pos_Old = 0.0f;
@@ -277,17 +264,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;
-int cnt_vel_findhome = 0;
-int FINDHOME_VELOCITY = 0;
-int FINDHOME_VELOCITY_OLD = 0;
-int FINDHOME_POSITION = 0;
-int FINDHOME_POSITION_OLD = 0;
-
int cnt_finddz = 0;
int cnt_vel_finddz = 0;
int flag_finddz = 0;
@@ -357,7 +333,9 @@
float PWM_out=0.0f;
-double K_v = 0.0f; // valve flowrate gain
+double K_v = 1.0f; // valve flowrate gain 1
+double C_d = 0.16f; // valve flowrate gain 2
+
double mV_PER_mA = 600.0f; // current >> voltage
double mV_PER_pulse = 0.6f; // pulse >> voltage
double mA_PER_pulse = 0.001f; // pulse >> current
@@ -453,8 +431,8 @@
PISTON_AREA_B = spi_eeprom_read(RID_PISTON_AREA_B);
PISTON_AREA_alpha = (float)PISTON_AREA_A/(float)PISTON_AREA_B;
alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
- PRES_SUPPLY = spi_eeprom_read(RID_PRES_SUPPLY);
- PRES_RETURN = spi_eeprom_read(RID_PRES_RETURN);
+ PRES_SUPPLY_NOM = spi_eeprom_read(RID_PRES_SUPPLY);
+ PRES_SUPPLY = PRES_SUPPLY_NOM;
ENC_LIMIT_MINUS = spi_eeprom_read(RID_ENC_LIMIT_MINUS);
ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
STROKE = spi_eeprom_read(RID_STROKE);
@@ -467,7 +445,7 @@
FRICTION = (float) (spi_eeprom_read(RID_FRICTION)) * 0.1f;
HOMEPOS_OFFSET = spi_eeprom_read(RID_HOMEPOS_OFFSET);
HOMEPOS_VALVE_OPENING = spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
- TORQUE_VREF = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_VREF)) *0.001f;
+ FORCE_VREF = (float) (spi_eeprom_read(RID_FORCE_SENSOR_VREF)) *0.001f;
PRES_A_VREF = (float) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) * 0.001f;
PRES_B_VREF = (float) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) * 0.001f;
VALVE_GAIN_LPM_PER_V[0] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_1)) * 0.01f;
@@ -497,62 +475,38 @@
/*******************************************************************************
* ENCODER functions
-
+
******************************************************************************/
-// A-KHA
-#define KF_G1_11 0.083920206005350f
-#define KF_G1_12 0.000013905329560f
-#define KF_G1_21 -0.000575742328210f
-#define KF_G1_22 0.799999939711725f
-// K
-#define KF_G2_11 0.916079793994650f
-#define KF_G2_12 0.000002878711641f
-#define KF_G2_21 0.000575742328210f
-#define KF_G2_22 0.199999945139809f
-
-float KF_Y_11 = 0.0f;
-float KF_Y_21 = 0.0f;
-float KF_X_11 = 0.0f;
-float KF_X_21 = 0.0f;
-
-long ENC_pos_old = 0, ENC_pos_cur = 0, ENC_pos_diff = 0;
-long ENC_RAW = 0, ENC_VEL_RAW = 0, ENC_VEL_KF = 0;
-long enc_offset = 0;
+long ENC_pulse = 0, ENC_pulse_old = 0, ENC_pulse_diff = 0;
+long ENC_pulse_offset = 0;
void ENC_UPDATE(void)
{
-
- ENC_pos_cur = spi_enc_read();
- ENC_pos_diff = ENC_pos_cur - ENC_pos_old;
-
- //Low Pass Filter
+ ENC_pulse = spi_enc_read(); // Unit : pulse
+ ENC_pulse_diff = ENC_pulse - ENC_pulse_old;
- 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);
+ pos.UpdateSen((float)((long)DIR_JOINT_ENC * ENC_pulse + ENC_pulse_offset)/ENC_PULSE_PER_POSITION, FREQ_10k, 100.0f); // Unit : deg or mm
+ vel.UpdateSen((float)((long)DIR_JOINT_ENC * ENC_pulse_diff * (long)FREQ_10k)/ENC_PULSE_PER_POSITION, FREQ_10k, 100.0f); // Unit : deg/s or mm/s
- double alpha_update_pos = 1.0f/(1.0f + FREQ_10k/(2.0f*3.14f*100.0f));
- pos.sen = NEW_POSITION;
- vel.sen = (1.0f - alpha_update_pos) * vel.sen + alpha_update_pos * NEW_VELOCITY; // pulse/s
-
- ENC_pos_old = ENC_pos_cur;
+ ENC_pulse_old = ENC_pulse;
}
void ENC_SET_ZERO(void)
{
-
spi_enc_set_clear();
- CUR_POSITION = 0;
- ENC_pos_old = ENC_pos_cur = 0;
-
+ pos.Reset();
+ ENC_pulse_offset = 0;
+ ENC_pulse = ENC_pulse_old = ENC_pulse_diff = 0;
}
-void ENC_SET(int32_t value_e)
+void ENC_SET(long value_e)
{
spi_enc_set_clear();
- enc_offset = value_e;
- CUR_POSITION = value_e;
- ENC_pos_old = ENC_pos_cur = value_e;
-
+ ENC_pulse_offset = value_e;
+ pos.Reset();
+ pos.sen = value_e/ENC_PULSE_PER_POSITION;
+ ENC_pulse = ENC_pulse_old = value_e;
+ ENC_pulse_diff = 0;
}
--- a/function_utilities/function_utilities.h Tue Apr 20 00:42:45 2021 +0000 +++ b/function_utilities/function_utilities.h Wed Apr 21 04:20:39 2021 +0000 @@ -28,7 +28,7 @@ void ENC_UPDATE(void); void ENC_SET_ZERO(void); -void ENC_SET(int32_t value); +void ENC_SET(long value); void VALVE_POSITION_INIT(void); void VALVE_PWM(int pwm, float vol_max, float vol_in);
--- a/main.cpp Tue Apr 20 00:42:45 2021 +0000
+++ b/main.cpp Wed Apr 21 04:20:39 2021 +0000
@@ -29,8 +29,8 @@
// dac & check ///////////////////////////////////////////
DigitalOut check(PC_2);
DigitalOut check_2(PC_3);
-AnalogOut dac_1(PA_4);
-AnalogOut dac_2(PA_5);
+AnalogOut dac_1(PA_4); // 0.0f ~ 1.0f
+AnalogOut dac_2(PA_5); // 0.0f ~ 1.0f
AnalogIn adc1(PC_4); //pressure_1
AnalogIn adc2(PB_0); //pressure_2
AnalogIn adc3(PC_1); //current
@@ -66,11 +66,12 @@
State pos;
State vel;
State Vout;
-State torq;
+State force;
+State torq; // unit : N
State torq_dot;
-State pres_A;
+State pres_A; // unit : bar
State pres_B;
-State cur;
+State cur; // unit : mA
State valve_pos;
State INIT_Vout;
@@ -91,26 +92,24 @@
extern int CID_TX_VALVE_POSITION;
extern int CID_TX_SOMETHING;
-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;
+inline float tanh_inv(float y) {
+ if(y >= 1.0f - 0.000001f) y = 1.0f - 0.000001f;
+ if(y <= -1.0f + 0.000001f) y = -1.0f + 0.000001f;
+ return log(sqrt((1.0f+y)/(1.0f-y)));
+}
+
/*******************************************************************************
* REFERENCE MODE
******************************************************************************/
enum _REFERENCE_MODE {
- MODE_REF_NO_ACT = 0, //0
- MODE_REF_DIRECT, //1
- MODE_REF_COS_INC, //2
- MODE_REF_LINE_INC, //3
- MODE_REF_SIN_WAVE, //4
- MODE_REF_SQUARE_WAVE, //5
+ MODE_REF_NO_ACT = 0,
+ MODE_REF_DIRECT,
+ MODE_REF_FINDHOME
};
/*******************************************************************************
@@ -234,7 +233,6 @@
make_delay();
-
// spi _ enc
spi_enc_set_init();
make_delay();
@@ -270,19 +268,14 @@
TIM3->CR1 ^= TIM_CR1_UDIS;
make_delay();
-
-
//Timer priority
NVIC_SetPriority(TIM3_IRQn, 2);
NVIC_SetPriority(TIM4_IRQn, 3);
-
-
-
//DAC init
if (SENSING_MODE == 0) {
- dac_1 = TORQUE_VREF / 3.3f;
+ dac_1 = FORCE_VREF / 3.3f;
dac_2 = 0.0f;
} else if (SENSING_MODE == 1) {
if (DIR_VALVE_ENC > 0) {
@@ -435,6 +428,9 @@
TIMER INTERRUPT
*******************************************************************************/
+//------------------------------------------------
+// TMR4 : Sensor Read & Data Handling
+//-----------------------------------------------
float FREQ_TMR4 = (float)FREQ_20k;
float DT_TMR4 = (float)DT_20k;
long CNT_TMR4 = 0;
@@ -442,52 +438,42 @@
extern "C" void TIM4_IRQHandler(void)
{
if (TIM4->SR & TIM_SR_UIF ) {
- /*******************************************************
- *** Sensor Read & Data Handling
- ********************************************************/
- //Encoder
+ // Current ===================================================
+ //ADC3->CR2 |= 0x40000000; // adc _ 12bit
+
+ cur.UpdateSen(((float)ADC3->DR-2047.5f)/2047.5f*10.0f, FREQ_TMR4, 500.0f); // unit : mA
+
+ // Encoder ===================================================
if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) {
ENC_UPDATE();
}
+ // Force or Pressure Transducer =============================================
ADC1->CR2 |= 0x40000000;
- // Torque Sensing =============================================
- if (SENSING_MODE == 0) {
- float pres_A_new = (((float) ADC1->DR) - 2047.5f);
- double alpha_update_ft = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 100.0f)); // f_cutoff : 100Hz
- pres_A.sen = (1.0f - alpha_update_ft) * pres_A.sen + alpha_update_ft * pres_A_new;
- torq.sen = -pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE;
-
- // Pressure Sensing (0~210)bar =============================================
- } else if (SENSING_MODE == 1) {
-
+ if (SENSING_MODE == 0) { // Force sensing
+ force.UpdateSen((((float)ADC1->DR) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR4, 100.0f); // unit : N
+ } else if (SENSING_MODE == 1) { // Pressure sensing
+ float pres_A_new, pres_B_new;
if (DIR_VALVE_ENC > 0) {
- pres_A_new = (((float)ADC1->DR) - PRES_A_NULL);
- pres_B_new = (((float)ADC2->DR) - PRES_B_NULL);
+ pres_A_new = (((float)ADC1->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
+ pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR;
} else {
- pres_A_new = (((float)ADC2->DR) - PRES_A_NULL);
- pres_B_new = (((float)ADC1->DR) - PRES_B_NULL);
+ pres_A_new = (((float)ADC2->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
+ pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR;
}
- double alpha_update_pres = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 200.0f)); // f_cutoff : 200Hz
- pres_A.sen = (1.0f - alpha_update_pres) * pres_A.sen + alpha_update_pres * pres_A_new;
- pres_B.sen = (1.0f - alpha_update_pres) * pres_B.sen + alpha_update_pres * pres_B_new;
- CUR_PRES_A_BAR = pres_A.sen / PRES_SENSOR_A_PULSE_PER_BAR;
- CUR_PRES_B_BAR = pres_B.sen / PRES_SENSOR_B_PULSE_PER_BAR;
+ pres_A.UpdateSen(pres_A_new,FREQ_TMR4,200.0f);
+ pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f);
if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
- torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.0001f; // mm^3*bar >> Nm
+ float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm
+ torq.UpdateSen(torq_new,FREQ_TMR4,1000.0); // unit : Nm
} else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
- torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.1f; // mm^2*bar >> N
+ float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N
+ force.UpdateSen(force_new,FREQ_TMR4,1000.0); // unit : N
}
}
- //Current
- //ADC3->CR2 |= 0x40000000; // adc _ 12bit
- 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);
-
CNT_TMR4++;
}
TIM4->SR = 0x0; // reset the status register
@@ -505,26 +491,15 @@
{
if (TIM3->SR & TIM_SR_UIF ) {
-
- if (((OPERATING_MODE&0b110)>>1) == 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) == 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
- }
-
if(MODE_POS_FT_TRANS == 1) {
+ if (alpha_trans == 1.0f) MODE_POS_FT_TRANS = 2;
alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
cnt_trans++;
torq.err_sum = 0;
if((float)cnt_trans * DT_TMR3 > 3.0f)
MODE_POS_FT_TRANS = 2;
} else if(MODE_POS_FT_TRANS == 3) {
+ if (alpha_trans == 0.0f) MODE_POS_FT_TRANS = 0;
alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
cnt_trans++;
torq.err_sum = 0;
@@ -539,22 +514,45 @@
}
-// Reference Loop ==========================================================
+ // Reference Update ==========================================================
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;
+ force.ref = REF_FORCE;
+ break;
+ }
+ case MODE_REF_FINDHOME: {
+ pos.ref = REF_POSITION_FINDHOME;
+ vel.ref = 0.0f;
+ torq.ref = 0.0f;
+ force.ref = 0.0f;
break;
}
default:
break;
}
+ if (((OPERATING_MODE&0b010)>>1) == 0) {
+ K_v = 1.03f; // Q = K_v*sqrt(deltaP)*tanh(C_d*Xv);
+ K_v = 0.16f;
+ 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&0b010)>>1) == 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
+ }
+
+ // =====================================================================
+ // CONTROL LOOP --------------------------------------------------------
+ // =====================================================================
int UTILITY_MODE = 0;
int CONTROL_MODE = 0;
@@ -565,350 +563,145 @@
CONTROL_MODE = CONTROL_UTILITY_MODE;
UTILITY_MODE = MODE_NO_ACT;
}
-
-
-
// UTILITY MODE ------------------------------------------------------------
switch (UTILITY_MODE) {
case MODE_NO_ACT: {
break;
}
- case MODE_TORQUE_SENSOR_NULLING: {
+ case MODE_TORQUE_SENSOR_NULLING:
+ {
+ static float FORCE_pulse_sum = 0.0;
+ static float PresA_pulse_sum = 0.0;
+ static float PresB_pulse_sum = 0.0;
+
// DAC Voltage reference set
- if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
- CUR_TORQUE_sum += torq.sen;
+ float VREF_TuningGain = 0.000003f;
+ if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 5) {
+ LED = 1;
+ if(SENSING_MODE == 0) { // Force Sensor (Loadcell)
+ FORCE_pulse_sum = FORCE_pulse_sum + force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE;
+ if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
+ float FORCE_pluse_mean = FORCE_pulse_sum / 10.0f;
+ FORCE_pulse_sum = 0.0f;
- if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
- CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0f;
- CUR_TORQUE_sum = 0;
-
- TORQUE_VREF += 0.000003f * (0.0f - CUR_TORQUE_mean);
+ FORCE_VREF += VREF_TuningGain * (0.0f - FORCE_pluse_mean);
+ if (FORCE_VREF > 3.3f) FORCE_VREF = 3.3f;
+ if (FORCE_VREF < 0.0f) FORCE_VREF = 0.0f;
+ dac_1 = FORCE_VREF / 3.3f;
+ }
+ } else if (SENSING_MODE == 1) { // Pressure Sensor
+ PresA_pulse_sum += pres_A.sen*PRES_SENSOR_A_PULSE_PER_BAR;
+ PresB_pulse_sum += pres_B.sen*PRES_SENSOR_B_PULSE_PER_BAR;
+ if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
+ float PresA_pluse_mean = PresA_pulse_sum / 10.0f;
+ float PresB_pluse_mean = PresB_pulse_sum / 10.0f;
+ PresA_pulse_sum = 0.0f;
+ PresB_pulse_sum = 0.0f;
- if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f;
- if (TORQUE_VREF < 0.0f) TORQUE_VREF = 0.0f;
- dac_1 = TORQUE_VREF / 3.3f;
+ PRES_A_VREF += VREF_TuningGain * (0.0f - PresA_pluse_mean);
+ if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
+ if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
+ dac_1 = PRES_A_VREF / 3.3f;
+ PRES_B_VREF += VREF_TuningGain * (0.0f - PresB_pluse_mean);
+ if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f;
+ if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
+ dac_2 = PRES_B_VREF / 3.3f;
+ }
}
+ TMR3_COUNT_TORQUE_NULL++;
} else {
+ if(SENSING_MODE == 0 ) { // Force Sensor (Loadcell)
+ FORCE_pulse_sum = 0.0f;
+ dac_1 = FORCE_VREF / 3.3f;
+ spi_eeprom_write(RID_FORCE_SENSOR_VREF, (int16_t)(FORCE_VREF * 1000.0f));
+ } else if (SENSING_MODE == 1) {
+ PresA_pulse_sum = 0.0f;
+ PresB_pulse_sum = 0.0f;
+ dac_1 = PRES_A_VREF / 3.3f;
+ dac_2 = PRES_B_VREF / 3.3f;
+ spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t)(PRES_A_VREF * 1000.0f));
+ spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t)(PRES_B_VREF * 1000.0f));
+ }
CONTROL_UTILITY_MODE = MODE_NO_ACT;
TMR3_COUNT_TORQUE_NULL = 0;
- CUR_TORQUE_sum = 0;
- CUR_TORQUE_mean = 0;
-
- spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0f));
-
- dac_1 = TORQUE_VREF / 3.3f;
-
}
- TMR3_COUNT_TORQUE_NULL++;
break;
}
- case MODE_FIND_HOME: {
+ case MODE_FIND_HOME:
+ {
+ static int cnt_findhome = 0;
+ static int cnt_terminate_findhome = 0;
+ static float FINDHOME_POSITION_pulse = 0.0f;
+ static float FINDHOME_POSITION_pulse_OLD = 0.0f;
+ static float FINDHOME_VELOCITY_pulse = 0.0f;
+ static float REF_POSITION_FINDHOME_INIT = 0.0f;
+
if (FINDHOME_STAGE == FINDHOME_INIT) {
- REFERENCE_MODE=MODE_REF_NO_ACT;
+ REFERENCE_MODE = MODE_REF_FINDHOME;
cnt_findhome = 0;
- cnt_vel_findhome = 0;
+ cnt_terminate_findhome = 0;
pos.ref = pos.sen;
vel.ref = 0.0f;
+ REF_POSITION_FINDHOME = pos.ref;
FINDHOME_STAGE = FINDHOME_GOTOLIMIT;
} else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) {
- int cnt_check_enc = (TMR_FREQ_5k/20);
+ int cnt_check_enc = (TMR_FREQ_5k/20); // 5000/20 = 250tic = 50msec
if(cnt_findhome%cnt_check_enc == 0) {
- FINDHOME_POSITION = pos.sen;
- FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD;
- FINDHOME_POSITION_OLD = FINDHOME_POSITION;
+ FINDHOME_POSITION_pulse = pos.sen*ENC_PULSE_PER_POSITION;
+ FINDHOME_VELOCITY_pulse = FINDHOME_POSITION_pulse - FINDHOME_POSITION_pulse_OLD;
+ FINDHOME_POSITION_pulse_OLD = FINDHOME_POSITION_pulse;
}
cnt_findhome++;
- if (abs(FINDHOME_VELOCITY) <= 1) {
- cnt_vel_findhome = cnt_vel_findhome + 1;
+ if (fabs(FINDHOME_VELOCITY_pulse) <= 1) {
+ cnt_terminate_findhome = cnt_terminate_findhome + 1;
} else {
- cnt_vel_findhome = 0;
+ cnt_terminate_findhome = 0;
}
- if ((cnt_vel_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec
+ if ((cnt_terminate_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec
+ double GOTOHOME_SPEED = 10.0f; // 20mm/s or 20deg/s
if (HOMEPOS_OFFSET > 0) {
- pos.ref = pos.ref + 12.0f;
+ REF_POSITION_FINDHOME = REF_POSITION_FINDHOME + GOTOHOME_SPEED*DT_5k;
} else {
- pos.ref = pos.ref - 12.0f;
+ REF_POSITION_FINDHOME = REF_POSITION_FINDHOME - GOTOHOME_SPEED*DT_5k;
}
-
-
CONTROL_MODE = MODE_JOINT_CONTROL;
alpha_trans = 0.0f;
-
} else {
- ENC_SET((int32_t)((int32_t)HOMEPOS_OFFSET*10));
- INIT_REF_POS = (int32_t)((int32_t)HOMEPOS_OFFSET*10);
- REF_POSITION = 0;
- REF_VELOCITY = 0;
- FINDHOME_POSITION = 0;
- FINDHOME_POSITION_OLD = 0;
- FINDHOME_VELOCITY = 0;
+ ENC_SET((long)((long)HOMEPOS_OFFSET*10));
+ REF_POSITION_FINDHOME_INIT = (float)((long)HOMEPOS_OFFSET*10);
+ FINDHOME_POSITION_pulse = 0;
+ FINDHOME_POSITION_pulse_OLD = 0;
+ FINDHOME_VELOCITY_pulse = 0;
+
cnt_findhome = 0;
- cnt_vel_findhome = 0;
+ cnt_terminate_findhome = 0;
+ pos.ref = 0.0f;
FINDHOME_STAGE = FINDHOME_ZEROPOSE;
-
-
- cnt_findhome = 0;
- pos.ref = 0.0f;
- vel.ref = 0.0f;
- pos.ref_home_pos = 0.0f;
- vel.ref_home_pos = 0.0f;
}
} else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) {
+
int T_move = 2*TMR_FREQ_5k;
- pos.ref = (0.0f - (float)INIT_REF_POS)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)INIT_REF_POS;
- vel.ref = 0.0f;
- alpha_trans = 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;
+ REF_POSITION_FINDHOME = (0.0f - REF_POSITION_FINDHOME_INIT)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)REF_POSITION_FINDHOME_INIT;
cnt_findhome++;
if (cnt_findhome >= T_move) {
cnt_findhome = 0;
pos.ref = 0.0f;
- vel.ref = 0.0f;
- pos.ref_home_pos = 0.0f;
- vel.ref_home_pos = 0.0f;
FINDHOME_STAGE = FINDHOME_INIT;
CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
- REFERENCE_MODE=MODE_REF_DIRECT;
- }
- }
-
- break;
- }
-
- case MODE_PRESSURE_SENSOR_NULLING: {
- // 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 * CUR_PRES_A_mean;
- PRES_B_VREF = PRES_B_VREF + VREF_NullingGain * 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;
-
- if (DIR_VALVE_ENC > 0) {
- dac_1 = PRES_A_VREF / 3.3f;
- dac_2 = PRES_B_VREF / 3.3f;
- } else {
- dac_1 = PRES_B_VREF / 3.3f;
- dac_2 = PRES_A_VREF / 3.3f;
- }
- }
- } else {
- CONTROL_UTILITY_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;
-
- spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0f));
- spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0f));
-
- if (DIR_VALVE_ENC > 0) {
- dac_1 = PRES_A_VREF / 3.3f;
- dac_2 = PRES_B_VREF / 3.3f;
- } else {
- dac_1 = PRES_B_VREF / 3.3f;
- dac_2 = PRES_A_VREF / 3.3f;
+ REFERENCE_MODE = MODE_REF_DIRECT;
}
}
- TMR3_COUNT_PRES_NULL++;
- 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;
-
- 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;
-
- } 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 (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;
-
-
- } 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;
-
- 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;
- }
-
- } else 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_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;
- }
- }
- 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);
- }
- pos.ref = temp_pos_ref + temp_pos_ref_offset;
-
- } 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;
}
-
- case MODE_SYSTEM_ID: {
- freq_sysid_Iref = (double) cnt_sysid * DT_TMR3 * 3.0f;
- 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;
- }
-
- case MODE_FREQ_TEST: {
- float valve_pos_ref = 2500.0f * sin(2.0f * 3.141592f * freq_test_valve_ref * (float) cnt_freq_test * DT_TMR3);
- if(valve_pos_ref >= 0) {
- valve_pos.ref = (double)VALVE_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_CENTER)/10000.0f;
- } else {
- valve_pos.ref = (double)VALVE_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_CENTER)/10000.0f;
- }
- ref_array[cnt_freq_test] = valve_pos_ref;
- if(value>=(float) VALVE_CENTER) {
- pos_array[cnt_freq_test] = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
- } else {
- pos_array[cnt_freq_test] = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
- }
-
- CONTROL_MODE = MODE_VALVE_POSITION_CONTROL;
- cnt_freq_test++;
- if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR3 > 2) {
- buffer_data_size = cnt_freq_test;
- cnt_freq_test = 0;
- cnt_send_buffer = 0;
- freq_test_valve_ref = freq_test_valve_ref * 1.05f;
- if (freq_test_valve_ref >= 400) {
- CONTROL_UTILITY_MODE = MODE_NO_ACT;
- CONTROL_MODE = MODE_NO_ACT;
- CAN_TX_PWM((int16_t) (1)); //1300
- }
- CONTROL_MODE = MODE_NO_ACT;
- CONTROL_UTILITY_MODE = MODE_SEND_OVER;
-
- }
- break;
- }
-
- case MODE_STEP_TEST: {
- float valve_pos_ref = 0.0f;
- if (cnt_step_test < (int) (1.0f * (float) TMR_FREQ_5k)) {
- valve_pos_ref = 0.0f;
- } else {
- valve_pos_ref = 10000.0f;
- }
- if(valve_pos_ref >= 0) {
- valve_pos.ref = (double)VALVE_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_CENTER)/10000.0f;
- } else {
- valve_pos.ref = (double)VALVE_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_CENTER)/10000.0f;
- }
- ref_array[cnt_step_test] = valve_pos_ref;
- if(value>=(float) VALVE_CENTER) {
- pos_array[cnt_step_test] = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
- } else {
- pos_array[cnt_step_test] = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
- }
-
- CONTROL_MODE = MODE_VALVE_POSITION_CONTROL;
- cnt_step_test++;
- if (cnt_step_test > (int) (2.0f * (float) TMR_FREQ_5k)) {
- buffer_data_size = cnt_step_test;
- cnt_step_test = 0;
- cnt_send_buffer = 0;
- CONTROL_UTILITY_MODE = MODE_SEND_OVER;
- CONTROL_MODE = MODE_NO_ACT;
- }
-
- break;
- }
-
default:
break;
}
-
-
// CONTROL MODE ------------------------------------------------------------
switch (CONTROL_MODE) {
case MODE_NO_ACT: {
@@ -921,125 +714,93 @@
VALVE_POS_CONTROL(valve_pos.ref);
V_out = Vout.ref;
} else if (CURRENT_CONTROL_MODE == 0) { //PWM
- V_out = valve_pos.ref;
+ V_out = valve_pos.ref;
} else {
- I_REF = valve_pos.ref * 0.001f;
+ I_REF = valve_pos.ref * 0.001f; // Unit : pulse >> mA
+ float I_MAX = 10.0f; // Max : 10mA
+ if (I_REF > I_MAX) {
+ I_REF = I_MAX;
+ } else if (I_REF < -I_MAX) {
+ I_REF = -I_MAX;
+ }
}
break;
}
- case MODE_JOINT_CONTROL: {
-
- 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]
-
- //K & D Low Pass Filter
- float alpha_K_D = 1.0f/(1.0f + 5000.0f/(2.0f*3.14f*30.0f)); // f_cutoff : 30Hz
- K_LPF = K_LPF*(1.0f-alpha_K_D)+K_SPRING*(alpha_K_D);
- D_LPF = D_LPF*(1.0f-alpha_K_D)+D_DAMPER*(alpha_K_D);
-
- torq_ref = torq.ref + K_LPF * pos.err - D_LPF * vel.sen / ENC_PULSE_PER_POSITION; //[N]
+ case MODE_JOINT_CONTROL:
+ {
- // torque feedback
- torq.err = torq_ref - torq.sen; //[N]
- torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
-
- 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
+ float temp_vel_pos = 0.0f; // desired velocity for position control
+ float temp_vel_FT = 0.0f; // desired velocity for force/torque control
+ float temp_vel_ff = 0.0f; // desired velocity for feedforward control
+ float temp_vel = 0.0f;
- 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) * PI / 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))));
-
- // velocity compensation for torque control
- if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
- I_REF_FORCE_FB = 0.001f * ((double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum);
- // temp_vel_torq = (0.01 * (double) VELOCITY_COMP_GAIN * (double) CUR_VELOCITY / (double) ENC_PULSE_PER_POSITION) * PI / 180.0; // rad/s
- temp_vel_torq = (0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / (double) ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s
- // L feedforward velocity
- } else if ((OPERATING_MODE & 0x01) == 1) {
- I_REF_FORCE_FB = 0.001f * 0.01f*((double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum); // Linear Actuators are more sensitive.
- // temp_vel_torq = (0.01 * (double) VELOCITY_COMP_GAIN * (double) CUR_VELOCITY / (double) ENC_PULSE_PER_POSITION); // mm/s
- temp_vel_torq = (0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / (double) ENC_PULSE_PER_POSITION); // mm/s
- // L feedforward velocity
- }
- if (temp_vel_torq > 0.0f) I_REF_VC = temp_vel_torq * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
- else I_REF_VC = temp_vel_torq * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
- // L velocity(rad/s or mm/s) >> I_ref(mA)
- // Ref_Joint_FT_dot = (Ref_Joint_FT_Nm - Ref_Joint_FT_Nm_old) / TMR_DT_5k;
- // Ref_Joint_FT_Nm_old = Ref_Joint_FT_Nm;
+ float wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
+
+ pos.err = pos.ref - pos.sen; // Unit : mm or deg
+ vel.err = vel.ref - vel.sen; // Unit : mm/s or deg/s
+
+ // position control command ===============================================================================================================================================
+ if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
+ temp_vel_pos = 0.01f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err) * PI / 180.0f; // rad/s
+ // L when P-gain = 100, f_cut = 10Hz
+ } else {
+ temp_vel_pos = 0.01f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err); // mm/s
+ // L when P-gain = 100, f_cut = 10Hz
+ }
- I_REF = (1.0f - alpha_trans) * I_REF_POS + alpha_trans * (I_REF_VC + I_REF_FORCE_FB);
-
- // Anti-windup for FT
- if (I_GAIN_JOINT_TORQUE != 0) {
- double I_MAX = 10.0f; // Maximum Current : 10mV
- double Ka = 2.0f / ((double) I_GAIN_JOINT_TORQUE * 0.001f);
- if (I_REF > I_MAX) {
- double I_rem = I_REF - I_MAX;
- I_rem = Ka*I_rem;
- I_REF = I_MAX;
- torq.err_sum = torq.err_sum - I_rem /(float) TMR_FREQ_5k;
- } else if (I_REF < -I_MAX) {
- double I_rem = I_REF - (-I_MAX);
- I_rem = Ka*I_rem;
- I_REF = -I_MAX;
- torq.err_sum = torq.err_sum - I_rem /(float) TMR_FREQ_5k;
- }
- }
-
+ // torque control command ===============================================================================================================================================
+ float alpha_SpringDamper = 1.0f/(1.0f+TMR_FREQ_5k/(2.0f*PI*30.0f));
+ K_LPF = alpha_SpringDamper * K_LPF + (1.0f-alpha_SpringDamper) * K_SPRING;
+ D_LPF = alpha_SpringDamper * D_LPF + (1.0f-alpha_SpringDamper) * D_DAMPER;
+
+ if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
+ float torq_ref_act = torq.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : Nm
+ torq.err = torq_ref_act - torq.sen;
+ torq.err_int += torq.err/((float)TMR_FREQ_5k);
+ temp_vel_FT = 0.0001f * (P_GAIN_JOINT_TORQUE * torq.err + I_GAIN_JOINT_TORQUE * torq.err_int); // Nm >> rad/s
} else {
- float VALVE_POS_RAW_FORCE_FB = 0.0f;
- float VALVE_POS_RAW_FORCE_FF = 0.0f;
- float VALVE_POS_RAW = 0.0f;
-
- VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * (torq.ref_diff - torq_dot.sen)) * 0.01f + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref));
-
- VALVE_POS_RAW_FORCE_FF = P_GAIN_JOINT_TORQUE_FF * torq_ref * 0.001f + D_GAIN_JOINT_TORQUE_FF * (torq_ref - torq_ref_past) * 0.0001f;
+ float force_ref_act = force.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : N
+ force.err = force_ref_act - force.sen;
+ force.err_int += force.err/((float)TMR_FREQ_5k);
+ temp_vel_FT = 0.0001f * (P_GAIN_JOINT_TORQUE * force.err + I_GAIN_JOINT_TORQUE * force.err_int); // N >> mm/s
+ }
+
+ // velocity feedforward command ========================================================================================================================================
+ if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
+ temp_vel_ff = 0.01f * (float)VELOCITY_COMP_GAIN * vel.ref * PI / 180.0f; // rad/s
+ } else {
+ temp_vel_ff = 0.01f * (float)VELOCITY_COMP_GAIN * vel.ref; // mm/s
+ }
- VALVE_POS_RAW = VALVE_POS_RAW_FORCE_FB + VALVE_POS_RAW_FORCE_FF;
-
-
- if (VALVE_POS_RAW >= 0) {
- valve_pos.ref = VALVE_POS_RAW + VALVE_DEADZONE_PLUS;
- } else {
- valve_pos.ref = VALVE_POS_RAW + VALVE_DEADZONE_MINUS;
+ // command integration =================================================================================================================================================
+ temp_vel = (1.0f - alpha_trans) * temp_vel_pos + alpha_trans * temp_vel_FT + temp_vel_ff; // Position Control + Torque Control + Velocity Feedforward
+
+ float Qact = 0.0f; // required flow rate
+ if( temp_vel > 0.0f ) {
+ Qact = temp_vel * ((float)PISTON_AREA_A * 0.00006f); // mm^3/sec >> LPM
+ I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f))))/C_d;
+ } else {
+ Qact = temp_vel * ((float)PISTON_AREA_B * 0.00006f); // mm^3/sec >> LPM
+ I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f))))/C_d;
+ }
+
+ // Anti-windup for FT
+ if (I_GAIN_JOINT_TORQUE != 0.0f) {
+ float I_MAX = 10.0f; // Maximum Current : 10mA
+ float Ka = 2.0f;
+ if (I_REF > I_MAX) {
+ float I_rem = I_REF - I_MAX;
+ I_REF = I_MAX;
+ float temp_vel_rem = K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_A * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
+ torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check!
+ } else if (I_REF < -I_MAX) {
+ double I_rem = I_REF - (-I_MAX);
+ I_REF = -I_MAX;
+ float temp_vel_rem = K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_B * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
+ torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check!
}
-
- if(I_GAIN_JOINT_TORQUE != 0) {
- double Ka = 2.0f / (double) I_GAIN_JOINT_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);
- V_out = (float) Vout.ref;
}
- torq_ref_past = torq_ref;
break;
}
@@ -1048,79 +809,78 @@
break;
}
- case MODE_JOINT_ADAPTIVE_BACKSTEPPING: {
-
-
- float Va = (1256.6f + Amm * pos.sen/(float)(ENC_PULSE_PER_POSITION)) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x, unit : m^3
- float Vb = (1256.6f + Amm * (79.0f - pos.sen/(float)(ENC_PULSE_PER_POSITION))) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x), unit : m^3
-
- V_adapt = 1.0f / (1.0f/Va + 1.0f/Vb); //initial 0.0000053f
-
- //float f3 = -Amm*Amm*beta*0.000001f*0.000001f/V_adapt * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076
- float f3_hat = -a_hat * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076
-
- float g3_prime = 0.0f;
- if (torq.sen > Amm*(Ps-Pt)*0.000001f) {
- g3_prime = 1.0f;
- } else if (torq.sen < -Amm*(Ps-Pt)*0.000001f) {
- g3_prime = -1.0f;
- } else {
- if ((value-VALVE_CENTER) > 0) {
- g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f);
-// g3_prime = sqrt(Ps-Pt);
- } else {
- g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f);
-// g3_prime = sqrt(Ps-Pt);
- }
- }
- float tau = 0.01f;
- float K_valve = 0.0004f;
-
- float x_v = 0.0f; //x_v : -1~1
- if(value>=VALVE_CENTER) {
- x_v = 1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
- } else {
- x_v = -1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
- }
- float f4 = -x_v/tau;
- float g4 = K_valve/tau;
-
- float torq_ref_dot = torq.ref_diff * 500.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]
-
- torq.err = torq.ref - torq.sen; //[N]
- torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
-
- float k3 = 2000.0f; //2000 //20000
- float k4 = 10.0f;
- float rho3 = 3.2f;
- float rho4 = 10000000.0f; //25000000.0f;
- float x_4_des = (-f3_hat + torq_ref_dot - k3*(-torq.err))/(gamma_hat*g3_prime);
- if (x_4_des > 1) x_4_des = 1;
- else if (x_4_des < -1) x_4_des = -1;
-
- if (x_4_des > 0) {
- valve_pos.ref = x_4_des * (float)(VALVE_MAX_POS - VALVE_CENTER) + (float) VALVE_CENTER;
- } else {
- valve_pos.ref = x_4_des * (float)(VALVE_CENTER - VALVE_MIN_POS) + (float) VALVE_CENTER;
- }
-
- float x_4_des_dot = (x_4_des - x_4_des_old)*(float) TMR_FREQ_5k;
- x_4_des_old = x_4_des;
- 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;
- float a_hat_dot = -rho3/rho_a*vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f*(-torq.err);
- a_hat = a_hat + a_hat_dot / (float) TMR_FREQ_5k;
-
- if(a_hat > -3000000.0f) a_hat = -3000000.0f;
- else if(a_hat < -30000000.0f) a_hat = -30000000.0f;
-
- break;
- }
+// case MODE_JOINT_ADAPTIVE_BACKSTEPPING: {
+//
+// float Va = (1256.6f + Amm * pos.sen/(float)(ENC_PULSE_PER_POSITION)) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x, unit : m^3
+// float Vb = (1256.6f + Amm * (79.0f - pos.sen/(float)(ENC_PULSE_PER_POSITION))) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x), unit : m^3
+//
+// V_adapt = 1.0f / (1.0f/Va + 1.0f/Vb); //initial 0.0000053f
+//
+// //float f3 = -Amm*Amm*beta*0.000001f*0.000001f/V_adapt * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076
+// float f3_hat = -a_hat * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076
+//
+// float g3_prime = 0.0f;
+// if (torq.sen > Amm*(Ps-Pt)*0.000001f) {
+// g3_prime = 1.0f;
+// } else if (torq.sen < -Amm*(Ps-Pt)*0.000001f) {
+// g3_prime = -1.0f;
+// } else {
+// if ((value-VALVE_CENTER) > 0) {
+// g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f);
+//// g3_prime = sqrt(Ps-Pt);
+// } else {
+// g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f);
+//// g3_prime = sqrt(Ps-Pt);
+// }
+// }
+// float tau = 0.01f;
+// float K_valve = 0.0004f;
+//
+// float x_v = 0.0f; //x_v : -1~1
+// if(value>=VALVE_CENTER) {
+// x_v = 1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+// } else {
+// x_v = -1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+// }
+// float f4 = -x_v/tau;
+// float g4 = K_valve/tau;
+//
+// float torq_ref_dot = torq.ref_diff * 500.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]
+//
+// torq.err = torq.ref - torq.sen; //[N]
+// torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
+//
+// float k3 = 2000.0f; //2000 //20000
+// float k4 = 10.0f;
+// float rho3 = 3.2f;
+// float rho4 = 10000000.0f; //25000000.0f;
+// float x_4_des = (-f3_hat + torq_ref_dot - k3*(-torq.err))/(gamma_hat*g3_prime);
+// if (x_4_des > 1) x_4_des = 1;
+// else if (x_4_des < -1) x_4_des = -1;
+//
+// if (x_4_des > 0) {
+// valve_pos.ref = x_4_des * (float)(VALVE_MAX_POS - VALVE_CENTER) + (float) VALVE_CENTER;
+// } else {
+// valve_pos.ref = x_4_des * (float)(VALVE_CENTER - VALVE_MIN_POS) + (float) VALVE_CENTER;
+// }
+//
+// float x_4_des_dot = (x_4_des - x_4_des_old)*(float) TMR_FREQ_5k;
+// x_4_des_old = x_4_des;
+// 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;
+// float a_hat_dot = -rho3/rho_a*vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f*(-torq.err);
+// a_hat = a_hat + a_hat_dot / (float) TMR_FREQ_5k;
+//
+// if(a_hat > -3000000.0f) a_hat = -3000000.0f;
+// else if(a_hat < -30000000.0f) a_hat = -30000000.0f;
+//
+// break;
+// }
default:
break;
@@ -1136,13 +896,10 @@
double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (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;
- if (FLAG_VALVE_DEADZONE) {
- if (I_REF_fil > 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_PLUS*mA_PER_pulse; // unit: mA
- else if (I_REF_fil < 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_MINUS*mA_PER_pulse; // unit: mA
- else I_REF_fil_DZ = I_REF_fil + (double)(VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f*mA_PER_pulse; // unit: mA
- } else {
- I_REF_fil_DZ = I_REF_fil;
- }
+ if (I_REF_fil > 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_PLUS*mA_PER_pulse; // unit: mA
+ else if (I_REF_fil < 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_MINUS*mA_PER_pulse; // unit: mA
+ else I_REF_fil_DZ = I_REF_fil + (double)(VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f*mA_PER_pulse; // unit: mA
+
I_ERR = I_REF_fil_DZ - (double)cur.sen;
I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f;
@@ -1184,10 +941,8 @@
VALVE_PWM_RAW = -V_MAX;
I_ERR_INT = I_ERR_INT - V_rem * 0.0002f;
}
- Cur_Valve_Open_pulse = cur.sen / mA_PER_pulse;
} else {
VALVE_PWM_RAW = I_REF * mV_PER_mA;
- Cur_Valve_Open_pulse = I_REF / mA_PER_pulse;
}
////////////////////////////////////////////////////////////////////////////
@@ -1202,6 +957,7 @@
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);
+
} else { //////////////////////////sw valve
// Output Voltage Linearization & Dead Zone Cancellation (Electrical dead-zone) by SW
if (V_out > 0 ) V_out = (V_out + 180.0f)/0.8588f;
@@ -1209,9 +965,9 @@
else V_out = 0.0f;
}
- /*******************************************************
- *** PWM
- ********************************************************/
+ ////////////////////////////////////////////////////////////////////
+ /////////////////// PWM Command ///////////////////////////////////
+ ////////////////////////////////////////////////////////////////////
if(DIR_VALVE<0) {
V_out = -V_out;
}
@@ -1239,38 +995,29 @@
TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
-
+ ////////////////////////////////////////////////////////////////////////////
+ ////////////////////// Data transmission through CAN //////////////////////
+ ////////////////////////////////////////////////////////////////////////////
+
if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
+
// 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), (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)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
- }
+ CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.1f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.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)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
- }
+ CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION*0.25f), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.01f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
}
}
- // PWM, ID:1300
+ // Valve Position (ID:1300)
if (flag_data_request[1] == HIGH) {
- CAN_TX_PWM((int16_t) (V_out)); //1300
+ CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
}
- // Reference Current, Current Current (ID:1400)
+ // Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400)
if (flag_data_request[2] == HIGH) {
- CAN_TX_CURRENT((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse)); // 1400
- }
-
- // Reference Current, Current Current (ID:1700)
- if (flag_data_request[3] == HIGH) {
- CAN_TX_SOMETHING((int16_t) (0), (int16_t) (0), (int16_t) (0), (int16_t) (0)); // 1700
+ CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (PRES_SUPPLY), (int16_t) (PRES_SUPPLY_NOM));
}
TMR2_COUNT_CAN_TX = 0;
--- a/setting.h Tue Apr 20 00:42:45 2021 +0000 +++ b/setting.h Wed Apr 21 04:20:39 2021 +0000 @@ -19,12 +19,15 @@ #define FREQ_20k 20000.0f #define FREQ_40k 40000.0f #define DT_500 0.002f -#define DT_1k 0.0005f +#define DT_1k 0.001f #define DT_5k 0.0002f #define DT_10k 0.0001f #define DT_20k 0.00005f #define DT_40k 0.000025f +//#define TMR_FREQ_10k 10000 +#define TMR_FREQ_5k 5000 + extern DigitalOut check; extern DigitalOut check_2; extern AnalogOut dac_1; @@ -79,9 +82,6 @@ #define SYSFREQ 200000000 #define PBCLK 100000000 -//#define TMR_FREQ_10k 10000 -#define TMR_FREQ_5k 5000 - #define FALSE 0 #define TRUE 1 #define OUTPUT 0 @@ -109,6 +109,8 @@ extern uint8_t CONTROL_MODE; extern uint8_t OPERATING_MODE; extern uint8_t SENSING_MODE; +extern uint8_t SUPPLY_PRESSURE_UPDATE; + extern uint8_t CONTROL_UTILITY_MODE; extern uint8_t CURRENT_CONTROL_MODE; extern uint8_t FLAG_VALVE_DEADZONE; @@ -169,16 +171,14 @@ extern float PISTON_AREA_alpha; extern float alpha3; - -extern int16_t PRES_SUPPLY; -extern int16_t PRES_RETURN; +extern float PRES_SUPPLY_NOM; +extern float PRES_SUPPLY; extern int16_t ENC_LIMIT_PLUS; extern int16_t ENC_LIMIT_MINUS; extern int16_t STROKE; - extern float Amm; extern float beta; extern float Ps; @@ -209,11 +209,13 @@ extern float DAC_REF; extern float DAC_RESOL; +extern float REF_FORCE; +extern float REF_TORQUE; extern float REF_POSITION; extern float REF_VELOCITY; -extern float REF_TORQUE; -extern float REF_TORQUE_OLD; -extern float REF_PRES_DIFF; + +extern float REF_POSITION_FINDHOME; + extern int16_t REF_PWM; extern int16_t REF_VALVE_POSITION; extern int16_t REF_CURRENT; @@ -221,19 +223,11 @@ extern int REF_MOVE_TIME_5k; extern int INIT_REF_PWM; extern int INIT_REF_VALVE_POS; -extern int32_t INIT_REF_POS; extern int INIT_REF_VEL; extern int INIT_REF_TORQUE; extern int INIT_REF_PRES_DIFF; extern int INIT_REF_CURRENT; -extern int CUR_POSITION; -extern int CUR_VELOCITY; -extern float CUR_TORQUE; -extern float CUR_PRES_A; -extern float CUR_PRES_B; -extern int CUR_VALVE_POSITION; - extern unsigned int TMR2_COUNT_LED1; extern unsigned int TMR2_COUNT_LED2; extern unsigned int TMR2_COUNT_CAN_TX; @@ -254,14 +248,12 @@ extern int cnt_buffer; extern float CUR_CURRENT_mA; -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 FORCE_VREF; extern float PRES_A_VREF; extern float PRES_B_VREF; -extern float TORQUE_VREF; extern float VALVE_PWM_RAW_FB; extern float VALVE_PWM_RAW_FF; @@ -303,11 +295,9 @@ extern float CUR_PRES_B_sum; extern float CUR_PRES_A_mean; extern float CUR_PRES_B_mean; -extern float CUR_TORQUE_sum; -extern float CUR_TORQUE_mean; -extern float PRES_A_NULL; -extern float PRES_B_NULL; -extern float TORQUE_NULL; +extern float PRES_A_NULL_pulse; +extern float PRES_B_NULL_pulse; +extern float FORCE_NULL_pulse; extern float Ref_Valve_Pos_Old; @@ -367,17 +357,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; -extern int cnt_vel_findhome; -extern int FINDHOME_VELOCITY; -extern int FINDHOME_VELOCITY_OLD; -extern int FINDHOME_POSITION; -extern int FINDHOME_POSITION_OLD; - extern int cnt_finddz; extern int cnt_vel_finddz; extern int flag_finddz; @@ -449,6 +428,7 @@ extern float PWM_out; extern double K_v; +extern double C_d; extern double mV_PER_mA; extern double mV_PER_pulse; extern double mA_PER_pulse;