Kim GiJeong
eeprom_test
Diff: main.cpp
- Revision:
- 45:35fa6884d0c6
- Parent:
- 44:fe7d5cfd2eea
- Child:
- 46:2694daea349b
diff -r fe7d5cfd2eea -r 35fa6884d0c6 main.cpp
--- a/main.cpp Tue Jan 14 09:11:20 2020 +0000
+++ b/main.cpp Thu Jan 16 12:05:15 2020 +0000
@@ -76,11 +76,7 @@
extern int CID_RX_CMD;
extern int CID_RX_REF_POSITION;
-extern int CID_RX_REF_TORQUE;
-extern int CID_RX_REF_PRES_DIFF;
-extern int CID_RX_REF_VOUT;
-extern int CID_RX_REF_VALVE_POSITION;
-extern int CID_RX_REF_CURRENT;
+extern int CID_RX_REF_PWM;
extern int CID_TX_INFO;
extern int CID_TX_POSITION;
@@ -114,7 +110,7 @@
MODE_VALVE_OPEN_LOOP, //1
MODE_VALVE_POSITION_CONTROL, //2
- MODE_JOINT_POSITION_TORQUE_CONTROL_PWM, //3
+ MODE_JOINT_CONTROL, //3
MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION, //4
MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5
@@ -269,9 +265,7 @@
Ref_Valve_Pos_FF = (float) VALVE_MIN_POS;
}
- //VELOCITY_COMP_GAIN = 20;
- //CAN_TX_PRES((int16_t)(Ref_Valve_Pos_FF), (int16_t) (7));
- Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER) + DDV_CENTER;
+ Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER);
return Ref_Valve_Pos_FF;
}
@@ -450,10 +444,34 @@
float FREQ_TMR3 = (float)FREQ_1k;
//float DT_TMR3 = (float)DT_5k;
float DT_TMR3 = (float)DT_1k;
+int cnt_trans = 0;
extern "C" void TIM3_IRQHandler(void)
{
if (TIM3->SR & TIM_SR_UIF ) {
ENC_UPDATE();
+
+ if(MODE_POS_FT_TRANS == 1){
+ alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans/(float)(3*TMR_FREQ_5k)))/2.0f;
+ cnt_trans++;
+ if(cnt_trans>3*TMR_FREQ_5k)
+ MODE_POS_FT_TRANS = 2;
+ }
+ if(MODE_POS_FT_TRANS == 3){
+ alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans/(float)(3*TMR_FREQ_5k)))/2.0f;
+ cnt_trans++;
+ if(cnt_trans>3*TMR_FREQ_5k)
+ MODE_POS_FT_TRANS = 0;
+ }
+ else if(MODE_POS_FT_TRANS == 2){
+ alpha_trans = 1.0;
+ cnt_trans = 0;
+ }
+ else{
+ alpha_trans = 0.0;
+ cnt_trans = 0;
+ }
+
+
// CONTROL LOOP ------------------------------------------------------------
@@ -473,42 +491,31 @@
break;
}
- case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: {
- float PWM_RAW_POS_FB = 0.0f; // PWM by Position Feedback
- float PWM_RAW_POS_FF = 0.0f; // PWM by Position Feedforward
- float PWM_RAW_FORCE_FB = 0.0f; // PWM by Force Feedback
-
- // 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;
- // PWM_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff;
- PWM_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err;
- PWM_RAW_POS_FB = PWM_RAW_POS_FB * 0.01f;
+ case MODE_JOINT_CONTROL: {
+
+ float VALVE_POS_RAW_FORCE_FB = 0.0f;
+
+ pos.err = pos.ref - pos.sen; //[pulse]
+ vel.err = vel.ref - vel.sen; //[pulse/s]
+ double K_spring = 1.0f; //[N/mm]
+ double D_damper = 0.0001f;
+ torq.ref = torq.ref + (K_spring * pos.err + D_damper * vel.err) / ENC_PULSE_PER_POSITION; //[N]
+
+ // 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;
+
- // feedforward input for position control
- float Ref_Vel_Act = vel.ref/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
- float K_ff = 0.9f;
- if(Ref_Vel_Act > 0) K_ff = 0.90f; // open
- if(Ref_Vel_Act > 0) K_ff = 0.75f; // close
- PWM_RAW_POS_FF = K_ff*Ref_Vel_Act/0.50f;
+ 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 * 0.01f + DDV_JOINT_POS_FF(vel.ref));
- // 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_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;
-
- PWM_RAW_FORCE_FB = 0.0f;
-
- V_out = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB;
+ if (VALVE_POS_RAW_FORCE_FB >= 0) {
+ valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
+ } else{
+ valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS;
+ }
+ VALVE_POS_CONTROL(valve_pos.ref);
break;
}
@@ -549,10 +556,10 @@
valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
- if (valve_pos.ref > DDV_CENTER) {
- valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS - DDV_CENTER;
- } else if(valve_pos.ref < DDV_CENTER) {
- valve_pos.ref = valve_pos.ref - DDV_CENTER + VALVE_DEADZONE_MINUS;
+ 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);
@@ -651,30 +658,6 @@
}
case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING: {
-
- float VALVE_POS_RAW_FORCE_FB = 0.0f;
-
- pos.err = pos.ref - pos.sen; //[pulse]
- vel.err = vel.ref - vel.sen; //[pulse/s]
- double K_spring = 1.0f;
- double D_damper = 0.0001f;
- torq.ref = (K_spring * pos.err + D_damper * vel.err) * ENC_PULSE_PER_POSITION; //[Nm]
-
- // torque feedback
- torq.err = torq.ref - torq.sen; //[pulse]
- torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse]
- if (torq.err_sum > 100) torq.err_sum = 100;
- if (torq.err_sum<-100) torq.err_sum = -100;
- VALVE_POS_RAW_FORCE_FB = ((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) * (float) TORQUE_SENSOR_PULSE_PER_TORQUE;
-
- valve_pos.ref = DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
-
- if (valve_pos.ref > DDV_CENTER) {
- valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS - DDV_CENTER;
- } else if(valve_pos.ref < DDV_CENTER) {
- valve_pos.ref = valve_pos.ref - DDV_CENTER + VALVE_DEADZONE_MINUS;
- }
- VALVE_POS_CONTROL(valve_pos.ref);
break;
}