1
Dependencies: mbed-dev-f303 FastPWM3
Revision 55:d614e29c60c5, committed 2021-04-14
- Comitter:
- shaorui
- Date:
- Wed Apr 14 11:46:16 2021 +0000
- Parent:
- 54:4c9415402628
- Commit message:
- 1
Changed in this revision
diff -r 4c9415402628 -r d614e29c60c5 Calibration/calibration.cpp --- a/Calibration/calibration.cpp Thu Sep 17 07:49:27 2020 +0000 +++ b/Calibration/calibration.cpp Wed Apr 14 11:46:16 2021 +0000 @@ -17,7 +17,8 @@ float theta_ref = 0; float theta_actual = 0; //float v_d = .15f; - float v_d = .20f; //Put all volts on the D-Axis + //float v_d = .20f; + float v_d = .60f; //Put all volts on the D-Axis float v_q = 0.0f; float v_u, v_v, v_w = 0; float dtc_u, dtc_v, dtc_w = .5f; @@ -90,7 +91,8 @@ float theta_ref = 0; float theta_actual = 0; //float v_d = .15f; - float v_d = .20f; // Put volts on the D-Axis + // float v_d = .20f; + float v_d = .60f; // Put volts on the D-Axis float v_q = 0.0f; float v_u, v_v, v_w = 0; float dtc_u, dtc_v, dtc_w = .5f; @@ -494,7 +496,8 @@ float theta_joint_actual = 0; //float v_d = .15f; - float v_d = .18f; // Put volts on the D-Axis + //float v_d = .18f; + float v_d = .60f; // Put volts on the D-Axis float v_q = 0.0f; float v_u, v_v, v_w = 0; float dtc_u, dtc_v, dtc_w = .5f;
diff -r 4c9415402628 -r d614e29c60c5 Config/hw_config.h --- a/Config/hw_config.h Thu Sep 17 07:49:27 2020 +0000 +++ b/Config/hw_config.h Wed Apr 14 11:46:16 2021 +0000 @@ -12,7 +12,7 @@ #define DTC_MIN 0.0f // Min phase duty cycle #define PWM_ARR 0x8CA /// timer autoreload value #define DTC_COMP .000f /// deadtime compensation (100 ns / 25 us) - +#define BRAKE PC_8 //used for electrical brake #endif
diff -r 4c9415402628 -r d614e29c60c5 hw_setup.cpp --- a/hw_setup.cpp Thu Sep 17 07:49:27 2020 +0000 +++ b/hw_setup.cpp Wed Apr 14 11:46:16 2021 +0000 @@ -77,5 +77,5 @@ Init_ADC(); gpio->led = new DigitalOut(LED); //Init_DAC(); - + gpio->brake=new PwmOut(BRAKE); } \ No newline at end of file
diff -r 4c9415402628 -r d614e29c60c5 main.cpp --- a/main.cpp Thu Sep 17 07:49:27 2020 +0000 +++ b/main.cpp Wed Apr 14 11:46:16 2021 +0000 @@ -60,6 +60,7 @@ volatile int count = 0; volatile int state = REST_MODE; volatile int state_change; +volatile int brake_count=0; volatile float Joint_init =0; //Joint intial angle volatile int J_M_flag = 0; // Joint motor angle combine @@ -119,6 +120,8 @@ printf(" esc - Exit to Menu\n\r"); wait_us(10); state_change = 0; + gpio.brake->write(0); //shut up electrical brake ---set up electrical brake 0% duty cycle, relative to period + brake_count=0; gpio.enable->write(0); gpio.led->write(0); } @@ -144,6 +147,8 @@ } void enter_torque_mode(void){ + // gpio.brake->write(0.8); + gpio.brake->write(1); controller.ovp_flag = 0; gpio.enable->write(1); // Enable gate drive reset_foc(&controller); // Tesets integrators, and other control loop parameters @@ -156,6 +161,7 @@ } void calibrate(void){ + gpio.brake->write(0.8); gpio.enable->write(1); // Enable gate drive gpio.led->write(1); // Turn on status LED order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering @@ -287,7 +293,7 @@ //////shaorui add for obtaining joint real position controller.theta_elec1 = ma700.GetElecPosition(); controller.theta_mech1 = (1.0f/GR)*ma700.GetMechPosition(); - controller.dtheta_mech1 =(1.0f/GR)*ma700.GetMechVelocity(); + controller.dtheta_mech1 =-(ma700.GetMechVelocity()+spi.GetMechVelocity()); controller.dtheta_elec1 = ma700.GetElecVelocity(); ///hjb add joint angle @@ -329,10 +335,12 @@ } } controller.theta_mech = controller.theta_joint_raw; + } else if(J_M_flag == 1) { controller.theta_mech = Joint_init + (1.0f/GR)*spi.GetMechPosition(); + } @@ -361,6 +369,7 @@ case CALIBRATION_MODE: // Run encoder calibration procedure if(state_change){ calibrate(); + gpio.brake->write(0); //shut up electrical brake ---set up electrical brake 0% duty cycle, relative to period } break; @@ -374,7 +383,7 @@ case TEST_TRAJECTORY_MODE: // Run encoder calibration procedure if(state_change){ test_jointround_flag=1; - J_M_flag = 0; + //J_M_flag = 0; stop_sign=0; jointcalibrate(); @@ -397,9 +406,14 @@ printf("OVP Triggered!\n\r"); } */ - + brake_count++; + if(brake_count>=3*40000) + { + //gpio.brake->write(0.125); //set up electrical brake 12.5% duty cycle, relative to period + gpio.brake->write(0.3); + } torque_control(&controller); - /* + if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ controller.i_d_ref = 0; controller.i_q_ref = 0; @@ -407,7 +421,7 @@ controller.kd = 0; controller.t_ff = 0; } - */ + commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop controller.timeout += 1; @@ -574,7 +588,7 @@ controller.mode = 0; controller.sidebct1=0; Init_All_HW(&gpio); // Setup PWM, ADC, GPIO - + gpio.brake->period_ms(25); //set up electrical brake 250 ms period wait(.1); gpio.enable->write(1); TIM1->CCR3 = PWM_ARR*(1.0f); // Write duty cycles @@ -660,80 +674,46 @@ // wait(.1); if(state == MOTOR_MODE) { - if(test_jointround_flag==1) - { + + + if(controller.theta_joint_raw*57.2957795<=1) { - //if(stop_sign==0) - //{ + controller.v_des = 0; wait(1); controller.p_des=0; - controller.v_des = 1.0f; + controller.v_des = 1.5f; controller.kp = 0; - controller.kd = 2.0f; + controller.kd = 5.0f; controller.t_ff = 0; wait(.5); - // } - /* - else - { - test_jointround_flag=0; - controller.v_des =0; - - } - */ } + else if(controller.theta_joint_raw*57.2957795>=(359)) { //stop_sign=1; controller.v_des = 0; wait(1); controller.p_des=0; - controller.v_des = -1.0f; + controller.v_des = -1.5f; controller.kp = 0; - controller.kd = 2.0f; + controller.kd = 5.0f; controller.t_ff = 0; wait(.5); printf("test position:%.3f\n\r",(1.0f/GR)* spi.GetMechPosition()); - } - } - //wait(.1); - // printf("%.3f\n\r",(1.0f/GR)* spi.GetMechPosition()); - // printf("%.3d, %.3d\n\r",test_jointround_flag, stop_sign); + } + - //printf("BCT: %.3x zzz: %.3x etxy: %.3x \n\r",ma700.Gettest(),ma700.Gettest1(),ma700.Gettest2()); - // float joint_mech_position=-(controller.theta_mech*360/(2.0f*PI)*GR+controller.theta_mech1*360/(2.0f*PI)); - // wucha1=(controller.theta_mech-controller.theta_mech1)*360/(2.0f*PI); - //wucha1=controller.theta_mech*360/(2.0f*PI)-joint_mech_position; - //wucha+=abs(wucha1); - //printf("M: %.3f J: %.3f E: %.3f EA: %.3f \n\r",controller.theta_mech*360/(2.0f*PI),controller.theta_mech1*360/(2.0f*PI),wucha1,float(wucha/i)) ; - // printf("M: %.3f J: %.3f E: %.3f EA: %.3f \n\r",controller.theta_mech*360/(2.0f*PI),joint_mech_position,wucha1,float(wucha/i)) ; - //printf("m_position: %.3f\n\r",controller.theta_mech*360/(2.0f*PI)*GR); - //printf("j_position: %.3f\n\r",controller.theta_mech1*360/(2.0f*PI)); - // float m_position=controller.theta_mech*57.2957795; - // float j_position=-controller.theta_mech1*360/(2.0f*PI)-controller.theta_mech*360/(2.0f*PI)*GR; - // float j_position=-(controller.theta_mech1+controller.theta_mech)*2807.49319614; - // float j_position=-controller.theta_mech1*57.2957795; - //printf("m:%.3f\n\r,j:%.3f\n\r",m_position,j_position); - if(count >= 4000){ -// printf("J: %.3f Mec: %.3f Jerr: %.3f JVerr: %.3f Kp: %.3f Kd: %.3f \n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, (controller.p_des - controller.theta_mech)*57.2957795,(controller.v_des - controller.dtheta_mech)*57.2957795, controller.kp,controller.kd); - //printf("Jraw:%.3f J: %.3f Mec: %.3f N: %.3d Nmod: %.3f Mmod: %.3f \n\r",controller.theta_joint_raw*57.2957795,controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, controller.Ncycle,controller.Ncycle_mod*57.2957795,controller.Mech_mod*57.2957795); - - printf("vd:%.3f vq: %.3f iq: %.3f Velc:%.3f\n\r",controller.v_d,controller.v_q,controller.i_q_ref,controller.dtheta_elec); - - printf("Pdes: %.3f Vdes: %.3f Kp: %.3f Kd: %.3f Tff: %.3f\n\r",controller.p_des*57.2957795, controller.v_des*57.2957795, controller.kp,controller.kd,controller.t_ff); - printf("Prel: %.3f Vrel: %.3f T: %.3f \n\r",controller.theta_mech*57.2957795, controller.dtheta_mech*57.2957795, controller.i_q_filt*KT_OUT); + if(count >= 40000){ + printf("J_init: %.3f Pref: %.3f preal: %.3f JM: %.3d one: %.3f two: %.3f\n\r",Joint_init*57.2957795,(1.0f/GR)*spi.GetMechPosition()*57.2957795,controller.theta_mech*57.2957795,J_M_flag ,controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR,(controller.theta_joint- controller.theta_joint_raw)*57.2957795); + // printf("Pref: %.3f qian: %.3f hou: %.3f M: %.3d \n\r", (1.0f/GR)*spi.GetMechPosition()*57.2957795,controller.theta_joint_raw*57.2957795,controller.theta_mech*57.2957795,J_M_flag); + // printf("v: %.3f v1: %.3f \n\r",controller.dtheta_mech*57.2957795,controller.dtheta_mech1*57.2957795); count = 0; } - - + i++; - //wait(.5); - } - - - } + } }
diff -r 4c9415402628 -r d614e29c60c5 mbed-dev.lib --- a/mbed-dev.lib Thu Sep 17 07:49:27 2020 +0000 +++ b/mbed-dev.lib Wed Apr 14 11:46:16 2021 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/benkatz/code/mbed-dev-f303/#902f8c6731d6 +https://os.mbed.com/users/benkatz/code/mbed-dev-f303/#36facd806e4a
diff -r 4c9415402628 -r d614e29c60c5 structs.h --- a/structs.h Thu Sep 17 07:49:27 2020 +0000 +++ b/structs.h Wed Apr 14 11:46:16 2021 +0000 @@ -10,6 +10,7 @@ DigitalOut *enable; DigitalOut *led; FastPWM *pwm_u, *pwm_v, *pwm_w; + PwmOut *brake; //used for brake } GPIOStruct; typedef struct{