Sungwoo Kim
Original Version of STM Board
Diff: main.cpp
- Revision:
- 34:bb2ca2fc2a8e
- Parent:
- 33:91b17819ec30
- Child:
- 35:34ce7b0347b8
diff -r 91b17819ec30 -r bb2ca2fc2a8e main.cpp
--- a/main.cpp Tue Nov 12 11:29:36 2019 +0000
+++ b/main.cpp Tue Nov 19 13:13:28 2019 +0000
@@ -192,15 +192,15 @@
can.attach(&CAN_RX_HANDLER);
CAN_ID_INIT();
make_delay();
-
+
//Timer priority
NVIC_SetPriority(TIM3_IRQn, 2);
NVIC_SetPriority(TIM2_IRQn, 3);
NVIC_SetPriority(TIM4_IRQn, 4);
-
+
//can.reset();
can.filter(msg.id, 0xFFFFF000, CANStandard);
-
+
// spi _ enc
spi_enc_set_init();
make_delay();
@@ -258,9 +258,9 @@
} else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
Ref_Valve_Pos_FF = (float) VALVE_MIN_POS;
}
-
+
Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER) + DDV_CENTER;
-
+
return Ref_Valve_Pos_FF;
}
@@ -283,8 +283,8 @@
} else if(REF_VALVE_POS < VALVE_MIN_POS) {
REF_VALVE_POS = VALVE_MIN_POS;
}
-
-
+
+
// if(REF_VALVE_POS >= VALVE_POS_VS_PWM[0])
// {
// if(REF_VALVE_POS <= VALVE_POS_VS_PWM[1]) {
@@ -329,19 +329,19 @@
#define LT_MAX_IDX 57
float LT_PWM_duty[LT_MAX_IDX] = {-100.0f, -80.0f, -60.0f, -50.0f, -40.0f, -35.0f, -30.0f, -25.0f, -20.0f,
- -19.0f, -18.0f, -17.0f, -16.0f, -15.0f, -14.0f, -13.0f, -12.0f, -11.0f, -10.0f,
- -9.0f, -8.0f, -7.0f, -6.0f, -5.0f, -4.0f, -3.0f, -2.0f, -1.0f, 0.0f,
- 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, 10.0f,
- 11.0f, 12.0f, 13.0f, 14.0f, 15.0f, 16.0f, 17.0f, 18.0f, 19.0f, 20.0f,
- 25.0f, 30.0f, 35.0f, 40.0f, 50.0f, 60.0f, 80.0f, 100.0f
- }; // duty
+ -19.0f, -18.0f, -17.0f, -16.0f, -15.0f, -14.0f, -13.0f, -12.0f, -11.0f, -10.0f,
+ -9.0f, -8.0f, -7.0f, -6.0f, -5.0f, -4.0f, -3.0f, -2.0f, -1.0f, 0.0f,
+ 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, 10.0f,
+ 11.0f, 12.0f, 13.0f, 14.0f, 15.0f, 16.0f, 17.0f, 18.0f, 19.0f, 20.0f,
+ 25.0f, 30.0f, 35.0f, 40.0f, 50.0f, 60.0f, 80.0f, 100.0f
+ }; // duty
float LT_Voltage_Output[LT_MAX_IDX] = {-321.4f, -291.3f, -261.5f, -246.8f, -231.7f, -223.9f, -216.1f, -207.9f, -198.8f,
- -196.9f, -195.0f, -192.5f, -188.8f, -184.5f, -180.2f, -175.9f, -171.5f, -166.3f, -161.0f,
- -156.0f, -149.5f, -139.0f, -126.0f, -107.0f, -87.5f, -64.0f, -38.5f, -9.4f, 0.0f,
- 12.0f, 43.5f, 69.0f, 94.0f, 114.0f, 132.0f, 146.0f, 155.5f, 162.3f, 168.2f,
- 173.1f, 178.2f, 182.8f, 187.4f, 191.8f, 196.0f, 199.7f, 201.9f, 203.8f, 205.6f,
- 214.6f, 222.5f, 230.4f, 238.2f, 253.3f, 268.0f, 297.6f, 327.7f
- }; // mV
+ -196.9f, -195.0f, -192.5f, -188.8f, -184.5f, -180.2f, -175.9f, -171.5f, -166.3f, -161.0f,
+ -156.0f, -149.5f, -139.0f, -126.0f, -107.0f, -87.5f, -64.0f, -38.5f, -9.4f, 0.0f,
+ 12.0f, 43.5f, 69.0f, 94.0f, 114.0f, 132.0f, 146.0f, 155.5f, 162.3f, 168.2f,
+ 173.1f, 178.2f, 182.8f, 187.4f, 191.8f, 196.0f, 199.7f, 201.9f, 203.8f, 205.6f,
+ 214.6f, 222.5f, 230.4f, 238.2f, 253.3f, 268.0f, 297.6f, 327.7f
+ }; // mV
float PWM_duty_byLT(float Ref_V)
{
@@ -499,8 +499,8 @@
}
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
@@ -537,18 +537,15 @@
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_CENTER;
-
- if (valve_pos.ref > DDV_CENTER)
- {
+
+ if (valve_pos.ref > DDV_CENTER) {
valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS - DDV_CENTER;
- }
- else if(valve_pos.ref < 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;
}
@@ -966,7 +963,7 @@
VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01f;
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
//I_REF = 0.04f*wn_Pos*((float)joint_pos_err/ENC_PULSE_PER_POSITION);
//// L velocity >> mA convert
@@ -1408,7 +1405,7 @@
VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
ROM_RESET_DATA();
-
+
CONTROL_MODE = MODE_NO_ACT;
DZ_index = 1;
}
@@ -1501,7 +1498,7 @@
first_check = 0;
VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
-
+
CONTROL_MODE = MODE_NO_ACT;
DZ_index = 1;
}
@@ -1625,10 +1622,9 @@
/*******************************************************
*** PWM
********************************************************/
- if (V_out >= 12000.0f){
+ if (V_out >= 12000.0f) {
V_out = 12000.0f;
- }
- else if(V_out<=-12000.0f){
+ } else if(V_out<=-12000.0f) {
V_out = -12000.0f;
}
PWM_out= V_out/SUPPLY_VOLTAGE; // Full duty : 12000.0mV
@@ -1758,3 +1754,4 @@
+