560 lines
12 KiB
C
560 lines
12 KiB
C
#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<pid_vel_timestamp)Ts = (float)(pid_vel_timestamp - now_us)/9*1e-6f;
|
||
else
|
||
Ts = (float)(0xFFFFFF - now_us + pid_vel_timestamp)/9*1e-6f;
|
||
pid_vel_timestamp = now_us;
|
||
if(Ts == 0 || Ts > 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<pid_ang_timestamp)Ts = (float)(pid_ang_timestamp - now_us)/9*1e-6f;
|
||
else
|
||
Ts = (float)(0xFFFFFF - now_us + pid_ang_timestamp)/9*1e-6f;
|
||
pid_ang_timestamp = now_us;
|
||
if(Ts == 0 || Ts > 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<pid_vel_timestamp)Ts = (float)(pid_vel_timestamp - now_us)/9*1e-6f;
|
||
else
|
||
Ts = (float)(0xFFFFFF - now_us + pid_vel_timestamp)/9*1e-6f;
|
||
pid_vel_timestamp = now_us;
|
||
if(Ts == 0 || Ts > 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;
|
||
}
|
||
|