first commit
This commit is contained in:
595
Gambal/User/main.c
Normal file
595
Gambal/User/main.c
Normal file
@ -0,0 +1,595 @@
|
||||
#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<><56><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4>ѹ
|
||||
int pole_pairs = 14; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
float voltage_limit = 7.8; //V,<2C><><EFBFBD><EFBFBD>ֵҪС<D2AA><D0A1>12/1.732=6.9
|
||||
float velocity_limit = 80; //rad/s <20>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
//0.6
|
||||
|
||||
/******************************************************************************/
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
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 <20><><EFBFBD><EFBFBD>
|
||||
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); //<2F>ٶȱջ<C8B1><D5BB><EFBFBD><EFBFBD><EFBFBD>
|
||||
void angle_close_loop(float new_target); //<2F>Ƕȱջ<C8B1><D5BB><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
void speed_open_loop(float new_target); //<2F>ٶȿ<D9B6><C8BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
|
||||
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); //<2F>ȴ<EFBFBD><C8B4><EFBFBD>Ƭ<EFBFBD><C6AC><EFBFBD>ȶ<EFBFBD>
|
||||
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 *)(x));
|
||||
|
||||
target = 0 - atof((const char *)(y));
|
||||
// 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=1.5; //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 =0.5;
|
||||
error_ang_prev= 0;
|
||||
pid_ang_timestamp=SysTick->VAL;
|
||||
|
||||
}
|
||||
//void commander_run(void)
|
||||
//{
|
||||
// if((USART_RX_STA&0x8000)!=0)
|
||||
// {
|
||||
// switch(USART_RX_BUF[0])
|
||||
// {
|
||||
|
||||
// case 'y': //D
|
||||
// target = 0 - atof((const char *)(USART_RX_BUF+1));
|
||||
// break;
|
||||
// case 'x':
|
||||
// target =atof((const char *)(USART_RX_BUF+1));
|
||||
// break;
|
||||
//
|
||||
// }
|
||||
|
||||
// USART_RX_STA=0;
|
||||
// }
|
||||
//}
|
||||
|
||||
/******************************************************************************/
|
||||
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;
|
||||
}
|
||||
/******************************************************************************/
|
||||
//void PID_init(void)
|
||||
//{
|
||||
// pid_vel_P=0.2; //0.1
|
||||
// pid_vel_I=2.0; //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=20;
|
||||
// pid_ang_D=0.5;
|
||||
// error_ang_prev=0;
|
||||
// pid_ang_timestamp=SysTick->VAL;
|
||||
//}
|
||||
/******************************************************************************/
|
||||
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;
|
||||
|
||||
|
||||
////////////////
|
||||
//<2F><>̬<EFBFBD><CCAC>:PD<50><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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;
|
||||
|
||||
//<2F><>ȡ<EFBFBD>ٶ<EFBFBD>
|
||||
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;
|
||||
|
||||
//<2F>ٶȻ<D9B6>
|
||||
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;
|
||||
|
||||
//<2F><><EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD>
|
||||
shaft_angle = shaftAngle();
|
||||
electrical_angle = electricalAngle();
|
||||
|
||||
setPhaseVoltage(output, 0, electrical_angle);
|
||||
// return output;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user