![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
rainbow
Diff: main.cpp
- Revision:
- 34:bb2ca2fc2a8e
- Parent:
- 33:91b17819ec30
- Child:
- 35:34ce7b0347b8
--- 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 @@ +