[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++;