#include "stm32f10x.h" #include "stm32f10x_it.h" #include "usart1.h" #include "pwm.h" #include "delay.h" #include "as5600.h" #include "foc_math.h" #include "led.h" #include "string.h" #include "stdlib.h" #include "stdio.h" float voltage_power_supply = 13.6; //V,电源电压 int pole_pairs = 14; //电机极数对 float voltage_limit = 7.8; //V,最大值要小于12/1.732=6.9 float velocity_limit = 80; //rad/s 速度限制 //0.6 /******************************************************************************/ //函数声明 float sensor_offset=0; float zero_electric_angle; float shaftVelocity(void); float electricalAngle(void); float shaftAngle(void); float y_vel_prev=0; float LPF_velocity(float x); u8 get_mpu6050(float *p); void close_loop(float x); void commander_run(void); //PID 参数 float pid_vel_P, pid_ang_P; float pid_vel_I, pid_ang_D; float integral_vel_prev; float error_vel_prev, error_ang_prev; float output_vel_ramp; float output_vel_prev; unsigned long pid_vel_timestamp, pid_ang_timestamp; void PID_init(void); float PID_angle(float error); float PID_velocity(float error); // /******************************************************************************/ void Motor_initFOC(void); float target=0; //float error; long sensor_direction = 1; unsigned long open_loop_timestamp; int alignSensor(void); float shaft_angle; float electrical_angle; float shaft_velocity; float current_sp; float shaft_velocity_sp; float shaft_angle_sp; void setPhaseVoltage(float Uq, float Ud, float angle_el); void speed_close_loop(float new_target); //速度闭环控制 void angle_close_loop(float new_target); //角度闭环控制 void speed_open_loop(float new_target); //速度开环控制 float error_zt_prev; void PID_ZT(float error); float y_ZT_prev=0; float LPF_ZT(float x); /******************************************************************************/ int main() { led_init(); led = 1; usart1_Init(2000000); delay_ms(100); TX_485; delay_ms(1000); //等待单片机稳定 delay_ms(1000); delay_ms(1000); led = 1; I2C_Init_(); TIM2_PWM_Init(); TIM4_1ms_Init(); MagneticSensor_Init(); M_Enable; Motor_initFOC(); PID_init(); systick_CountMode(); RX_485; target =0; time1_cntr = 0; time2_cntr = 0; USART_RX_STA=0; memset(USART_RX_BUF,0,64); // while(time1_cntr < 13000) // { //// PID_ZT(0); // speed_close_loop(3.8); //// angle_close_loop(6.0); //// angle_close_loop(2.4); // commander_run(); // } while(1) { // setPhaseVoltage(6,0,0); PID_ZT(target); // target =atof((const char *)(G_RX_Xerror)); target = 0 - atof((const char *)(G_RX_Yerror)); // target =LPF_ZT(target); // angle_close_loop(3.8); // close_loop(target); // commander_run(); // speed_close_loop(_PI); // i++; // shaft_angle = shaftAngle(); // printf("A%.3f",shaft_angle); } } void PID_init(void) { // ///xxxxxxxxxxxxxx // pid_vel_P=0.01; //0.1 // pid_vel_I=0.12; //2 // output_vel_ramp=100; // integral_vel_prev=0; // error_vel_prev=0; // output_vel_prev=0; // pid_vel_timestamp=SysTick->VAL; // pid_ang_P = 120; // pid_ang_D = 10; // error_ang_prev= 0; // pid_ang_timestamp=SysTick->VAL; // ////////yyyyyyyyyyyy pid_vel_P=0.8; //0.1 pid_vel_I=0.1; //2 output_vel_ramp=100; integral_vel_prev=0; error_vel_prev=0; output_vel_prev=0; pid_vel_timestamp=SysTick->VAL; pid_ang_P = 10; pid_ang_D =5; error_ang_prev= 0; pid_ang_timestamp=SysTick->VAL; } /******************************************************************************/ float shaftAngle(void) { return sensor_direction*getAngle() - sensor_offset; } float shaftVelocity(void) { return sensor_direction*LPF_velocity(getVelocity()); } /******************************************************************************/ float electricalAngle(void) { return _normalizeAngle((shaft_angle + sensor_offset) * pole_pairs - zero_electric_angle); } float LPF_ZT(float x) { float y = 0.01*y_vel_prev + 0.99*x; y_ZT_prev=y; return y; } float LPF_velocity(float x) { float y = 0.9*y_vel_prev + 0.1*x; y_vel_prev=y; return y; } /******************************************************************************/ float PID_velocity(float error) { unsigned long now_us; float Ts; float proportional,integral,output; float output_rate; now_us = SysTick->VAL; if(now_us 0.5) Ts = 1e-3f; proportional = pid_vel_P * error; integral = integral_vel_prev + pid_vel_I*Ts*0.5*(error + error_vel_prev); integral = _constrain(integral, -voltage_limit, voltage_limit); output = proportional + integral; output = _constrain(output, -voltage_limit, voltage_limit); output_rate = (output - output_vel_prev)/Ts; if(output_rate > output_vel_ramp)output = output_vel_prev + output_vel_ramp*Ts; else if(output_rate < -output_vel_ramp)output = output_vel_prev - output_vel_ramp*Ts; output = _constrain(output, -voltage_limit, voltage_limit); integral_vel_prev = integral; output_vel_prev = output; error_vel_prev = error; return output; } float PID_angle(float error) { unsigned long now_us; float Ts; float proportional,derivative,output; now_us = SysTick->VAL; if(now_us 0.5) Ts = 1e-3f; proportional = pid_ang_P * error; derivative = pid_ang_D*(error - error_ang_prev)/Ts; output = proportional + derivative; output = _constrain(output, -velocity_limit, velocity_limit); error_ang_prev = error; return output; } void Motor_initFOC(void) { // alignSensor(); angle_prev=getAngle(); delay_ms(5); shaft_velocity = shaftVelocity(); delay_ms(5); shaft_angle = shaftAngle(); delay_ms(200); } int alignSensor(void) { // long i; // float angle; // float mid_angle,end_angle; // float moved; // //// printf("MOT: Align sensor.\r\n"); // // setPhaseVoltage(2.5, 0, _3PI_2 ); // delay_ms(100); // mid_angle=getAngle(); // for(i=0; i<=500; i++) // { // angle = _3PI_2 + _2PI * i / 500.0; // setPhaseVoltage(2.5, 0, angle); // delay_ms(2); // } // end_angle=getAngle(); // // for(i=500; i>=0; i--) // { // angle = _3PI_2 + _2PI * i / 500.0 ; // setPhaseVoltage(2.5, 0, angle); // delay_ms(2); // } // setPhaseVoltage(0, 0, 0); // delay_ms(200); // //// printf("mid_angle=%.4f\n",mid_angle); //// printf("end_angle=%.4f\n",end_angle); // // moved = fabs(end_angle - mid_angle); // if((mid_angle == end_angle)||(moved < 0.02)) // { //// printf("Moto Failed Init.\n"); // M_Disable; // return 0; // } // else if(mid_angle < end_angle) // { //// printf("Moto sensor_direction== 1\n"); // sensor_direction=1; // } // else // { //// printf("Moto sensor_direction== -1\n"); // sensor_direction= -1; // } // // printf("MOT: PP check:\n "); // if( fabs(moved*pole_pairs - _2PI) > 0.5 ) // { // printf("fail - estimated pp:"); // pole_pairs =_2PI/moved+0.5; // printf("%d\n",pole_pairs); // } // else // printf("OK!\n"); setPhaseVoltage(3, 0, _3PI_2); delay_ms(700); // zero_electric_angle = 0; zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*getAngle(), pole_pairs)); delay_ms(20); // printf("MOT: Zero elec. angle:"); // printf("%.4f\r\n",zero_electric_angle); // zero_electric_angle = 0; setPhaseVoltage(0, 0, 0); delay_ms(100); return 1; } void setPhaseVoltage(float Uq, float Ud, float angle_el) { float Uout; uint32_t sector; float T0,T1,T2; float Ta,Tb,Tc; if(Ud) { Uout = _sqrt(Ud*Ud + Uq*Uq) / voltage_power_supply; angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud)); } else { Uout = Uq / voltage_power_supply; angle_el = _normalizeAngle(angle_el + _PI_2); } if(Uout> 0.577)Uout= 0.577; if(Uout<-0.577)Uout=-0.577; sector = (angle_el / _PI_3) + 1; T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout; T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uout; T0 = 1 - T1 - T2; switch(sector) { case 1: Ta = T1 + T2 + T0/2; Tb = T2 + T0/2; Tc = T0/2; break; case 2: Ta = T1 + T0/2; Tb = T1 + T2 + T0/2; Tc = T0/2; break; case 3: Ta = T0/2; Tb = T1 + T2 + T0/2; Tc = T2 + T0/2; break; case 4: Ta = T0/2; Tb = T1+ T0/2; Tc = T1 + T2 + T0/2; break; case 5: Ta = T2 + T0/2; Tb = T0/2; Tc = T1 + T2 + T0/2; break; case 6: Ta = T1 + T2 + T0/2; Tb = T0/2; Tc = T1 + T0/2; break; default: Ta = 0; Tb = 0; Tc = 0; } TIM_SetCompare1(TIM2,Ta*1440); TIM_SetCompare2(TIM2,Tb*1440); TIM_SetCompare3(TIM2,Tc*1440); } void speed_close_loop(float new_target) { float Uq,Ud,electrical_angle; shaft_velocity = shaftVelocity(); shaft_velocity_sp = new_target; current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); Uq = current_sp; Ud = 0; shaft_angle = shaftAngle(); electrical_angle = electricalAngle(); setPhaseVoltage(Uq, Ud, electrical_angle); } void angle_close_loop(float new_target) { float Uq,Ud,electrical_angle; shaft_velocity = shaftVelocity(); shaft_angle_sp = new_target; // shaft_angle = shaftAngle(); // shaft_velocity_sp = PID_angle(shaft_angle_sp - shaft_angle); current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); Uq = current_sp; Ud = 0; // shaft_angle = shaftAngle(); electrical_angle = electricalAngle(); setPhaseVoltage(Uq, Ud, electrical_angle); } void speed_open_loop(float new_target) { float Uq = new_target; Uq =_constrain(Uq,-5,5); for(float electrical_angle = 0;electrical_angle < _2PI;electrical_angle = electrical_angle + 0.1) { setPhaseVoltage(Uq, 0, electrical_angle); } } void close_loop(float e) { float Uq,Ud,electrical_angle; shaft_velocity = shaftVelocity(); shaft_velocity_sp = PID_angle(e); current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); Uq = current_sp; Ud = 0; shaft_angle = shaftAngle(); electrical_angle = electricalAngle(); setPhaseVoltage(Uq, Ud, electrical_angle); } void PID_ZT(float error) { unsigned long now_us; float Ts; float proportional,integral,output; float output_rate; now_us = SysTick->VAL; if(now_us 0.5) Ts = 1e-3f; //////////////// //姿态环:PD控制算出target_v proportional = pid_ang_P * error; float derivative = pid_ang_D*(error - error_zt_prev)/Ts; float target_v = proportional + derivative; target_v = _constrain(target_v, -velocity_limit, velocity_limit); error_zt_prev = error; //获取速度 float now_angle,now_v; now_angle = getAngle(); //now_v now_v = (now_angle - angle_prev)/Ts; now_v = LPF_velocity(now_v); angle_prev = now_angle; velocity_calc_timestamp = now_us; //速度环 error = target_v - now_v; proportional = pid_vel_P * error; integral = integral_vel_prev + pid_vel_I*Ts*0.5*(error + error_vel_prev); integral = _constrain(integral, -voltage_limit, voltage_limit); output = proportional+integral; output = _constrain(output, -voltage_limit, voltage_limit); output_rate = (output - output_vel_prev)/Ts; if(output_rate > output_vel_ramp)output = output_vel_prev + output_vel_ramp*Ts; else if(output_rate < -output_vel_ramp)output = output_vel_prev - output_vel_ramp*Ts; output = _constrain(output, -voltage_limit, voltage_limit); integral_vel_prev = integral; output_vel_prev = output; error_vel_prev = error; //控制电机 shaft_angle = shaftAngle(); electrical_angle = electricalAngle(); setPhaseVoltage(output, 0, electrical_angle); // return output; }