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).
Diff: main.cpp
- Revision:
- 53:4d66fb1c5dd9
- Parent:
- 52:8ea76864368a
diff -r 8ea76864368a -r 4d66fb1c5dd9 main.cpp
--- a/main.cpp Wed Feb 19 05:48:57 2020 +0000
+++ b/main.cpp Tue Feb 25 07:39:17 2020 +0000
@@ -192,7 +192,7 @@
//Timer priority
NVIC_SetPriority(TIM3_IRQn, 2);
// NVIC_SetPriority(TIM2_IRQn, 3);
- NVIC_SetPriority(TIM4_IRQn, 4);
+ NVIC_SetPriority(TIM4_IRQn, 3);
//can.reset();
can.filter(msg.id, 0xFFFFF000, CANStandard);
@@ -405,11 +405,12 @@
int j =0;
//unsigned long CNT_TMR3 = 0;
//float FREQ_TMR3 = (float)FREQ_5k;
-float FREQ_TMR3 = (float)FREQ_1k;
+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 canfreq = CAN_FREQUENCY;
extern "C" void TIM3_IRQHandler(void)
{
@@ -455,10 +456,14 @@
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]
- double torq_ref = 0.0f;
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]
@@ -493,9 +498,12 @@
}
VALVE_POS_CONTROL(valve_pos.ref);
-
+
+ //TMR3_COUNT_TEST++;
+
break;
}
+
case MODE_VALVE_OPEN_LOOP: {
V_out = (float) Vout.ref;
@@ -1634,57 +1642,61 @@
- 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 & 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 & 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) {
- CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (pres_A.sen*10.0f), (int16_t) (pres_B.sen*10.0f));
+ 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] == 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 & 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) {
+ CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (pres_A.sen*10.0f), (int16_t) (pres_B.sen*10.0f));
+ }
}
}
- }
- if (flag_data_request[1] == HIGH) {
- //valve position
- double t_value = 0;
- 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)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
+ if (flag_data_request[1] == HIGH) {
+ //valve position
+ double t_value = 0;
+ 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)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
+ }
+ CAN_TX_TORQUE((int16_t) (t_value));
}
- CAN_TX_TORQUE((int16_t) (t_value));
- }
- 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[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] == HIGH) {
- //PWM
- CAN_TX_PWM((int16_t) VALVE_DEADZONE_PLUS);
- }
+ if (flag_data_request[3] == HIGH) {
+ //PWM
+ CAN_TX_PWM((int16_t) VALVE_DEADZONE_PLUS);
+ }
- 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);
- }
+ 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);
+ }
- // Others : Reference position, Reference FT, PWM, Current (ID:1300)
+ // Others : Reference position, Reference FT, PWM, Current (ID:1300)
// if (flag_data_request[1] == HIGH) {
// CAN_TX_SOMETHING((int) (FORCE_VREF), (int16_t) (1), (int16_t) (2), (int16_t) (3));
// }
+ //if (flag_delay_test == 1){
+ //CAN_TX_PRES((int16_t) (0),(int16_t) torq_ref);
+ //}
- TMR2_COUNT_CAN_TX = 0;
- }
+ TMR2_COUNT_CAN_TX = 0;
+ }
+ TMR2_COUNT_CAN_TX++;