# 准备工作

## 增量式以及位置式PID

float PID_Control_Inc(PID* pid, int flag) // 增量式PID
{
float inc = 0;

pid->ek = pid->SetValue - pid->ActualValue;

inc = pid->KP * (pid->ek - pid->ek_1) + pid->KI * pid->ek
+ pid->KD * (pid->ek - 2 * pid->ek_1 + pid->ek_2);

pid->ek_2 = pid->ek_1; //存储误差
pid->ek_1 = pid->ek; //存储误差

if(flag == 1)
{
if(inc > pid->PIDmax)
inc = pid->PIDmax;
if(inc < pid->PIDmin)
inc = pid->PIDmin;
}
pid->PIDout = inc;
return pid->PIDout;
}
float PID_Control_Pos(PID* pid, int flag) // 位置式PID
{
float Pos = 0;

pid->ek = pid->SetValue - pid->ActualValue;
pid->ek_sum += pid->ek;

if(pid->ek_sum > pid->Sum_max)
pid->ek_sum = pid->Sum_max;
if(pid->ek_sum < pid->Sum_min)
pid->ek_sum = pid->Sum_min;

Pos = pid->KP * pid->ek + pid->KI * pid->ek_sum + pid->KD * (pid->ek - pid->ek_1);

pid->ek_2 = pid->ek_1; //存储误差
pid->ek_1 = pid->ek; //存储误差

// 积分限幅
if(flag == 1)
{
if(Pos > pid->PIDmax)
Pos = pid->PIDmax;
if(Pos < pid->PIDmin)
Pos = pid->PIDmin;
}
pid->PIDout = Pos;

return pid->PIDout;
}
typedef struct // PID结构体定义
{
float32 SetValue; // 期望值 参考值
float32 ActualValue; // 实际值
float32 KP; // 比例因子
float32 KI; // 积分因子
float32 KD; // 微分因子
float32 ek; // 本级误差
float32 ek_1; // 上一次
float32 ek_2; // 上上次
float32 ek_sum; // 误差累积

float32 Sum_max; // 误差累和上限
float32 Sum_min; // 误差累和下限
float32 PIDmax; // max limit
float32 PIDmin; // min limit

float32 PIDout; // output
}PID;


## 电机闭环

P

u

l

s

e

2

M

P

S

=

1

512

×

31

68

×

1

0

3

×

π

×

500

=

0.0895

Pulse2MPS=frac{1}{512}times frac{31}{68}times 10^{-3} times pi times 500=0.0895

Pulse2MPS=5121×6831×103×π×500=0.0895

extern PID pid_motor_L = {0}; // 左轮
extern PID pid_motor_R = {0}; // 右轮


void TIM1_Isr interrupt 3()
{
float inc_L = 0.0;
float inc_R = 0.0;
float temp_L = 0.0;
float temp_R = 0.0;
float speed_goal = 1.5 * 60; // 后轮速度，米每分

temp_L = ctimer_count_read(Encoder_L) * Pulse2MPM; // 左右轮当前速度

ctimer_count_clean(Encoder_L); // 编码器清零
ctimer_count_clean(Encoder_R);

if(DIR_Encoder_L == 1)
pid_motor_L.ActualValue = temp_L;
else
pid_motor_L.ActualValue = (-1) * temp_L;
if(DIR_Encoder_R == 0)
pid_motor_R.ActualValue = temp_R;
else
pid_motor_R.ActualValue = (-1) * temp_R;

pid_motor_L.SetValue = speed_goal ; // 差速
pid_motor_R.SetValue = speed_goal ;
inc_L = PID_Control_Inc(&pid_motor_L, 1);
inc_R = PID_Control_Inc(&pid_motor_R, 1);

duty_L += inc_L;
duty_R += inc_R;

if(duty_L > Motor_UpperLimit)
duty_L = Motor_UpperLimit;
if(duty_L < Motor_LowerLimit)
duty_L = Motor_LowerLimit;

if(duty_R > Motor_UpperLimit)
duty_R = Motor_UpperLimit;
if(duty_R < Motor_LowerLimit)
duty_R = Motor_LowerLimit;

DIR_1 = 0;
pwm_duty(Motor_L, duty_L);
DIR_2 = 0;
pwm_duty(Motor_R, duty_R);
}


## 电磁采样

e

r

r

o

r

1

=

1

/

L

r

e

s

1

/

M

r

e

s

1

/

L

r

e

s

+

1

/

M

r

e

s

,

e

r

r

o

r

2

=

1

/

M

r

e

s

1

/

R

r

e

s

1

/

M

r

e

s

+

1

/

R

r

e

s

error_1=frac{1/Lres-1/Mres}{1/Lres+1/Mres}, error_2=frac{1/Mres-1/Rres}{1/Mres+1/Rres}

error1=1/Lres+1/Mres1/Lres1/Mres,error2=1/Mres+1/Rres1/Mres1/Rres

e

r

r

o

r

=

e

r

r

o

r

1

+

e

r

r

o

r

2

e

r

r

o

r

1

e

r

r

o

r

2

error=frac{error_1+error_2}{error_1-error_2}

error=error1error2error1+error2

// sample

// get sum
Sum_L  = L[0] + L[1] + L[2] + L[3] + L[4];
Sum_R  = R[0] + R[1] + R[2] + R[3] + R[4];
Sum_M  = M[0] + M[1] + M[2] + M[3] + M[4];

// normalize and mean filter

error1 = (1.0 / Lres - 1.0 / Mres) / (1.0 / Lres + 1.0 / Mres);
error2 = (1.0 / Mres - 1.0 / Rres) / (1.0 / Mres + 1.0 / Rres);
error = (error1 + error2) / (error1 + error2);


## 舵机闭环

void TIM1_Isr interrupt 3()
{
pid_steer.ActualValue = error;
temp = PID_Control_Pos(&pid_steer, 1);

duty_steer = Middle + 1.0 * (uint16)(temp);

if(duty_steer > Left) // 舵机限幅
duty_steer = Left;
if(duty_steer < Right) // 舵机限幅
duty_steer = Right;

pwm_duty(PWMB_CH1_P74, duty_steer);
}


## 合并

uint16 const Middle = 800;
uint16 const Left = 880;
uint16 const Right = 720;

float error1,error2, error = 0;

uint16 duty_steer;
uint16 duty_L = 1500;
uint16 duty_R = 1500;
uint8 flag_fork=0; // 三叉标志
uint8 flag_R_circ_pre,flag_R_circ_in,flag_R_circ_out=0; // 右环岛
uint8 flag_L_circ_pre,flag_L_circ_in,flag_L_circ_out=0; // 左环岛
uint8 flag_trackout = 0;
uint8 Debug = 0;
uint8 test = 0;
float32 L[5], R[5], M[5]= {0}; // 存储以滤波
float32 Lres, Rres, Mres = 0; // 滤波结果，真正用于计算
float32 speed_goal = 1.5 * 60;

void TM1_Isr() interrupt 3
{

static uint16 count_led = 0; // 中断计数用于点灯
static uint16 count_sample = 0;  // 中断计数用于滤波
static uint16 count_delay_fork=0; // 三叉判断延时
static uint16 count_delay_R_circ_pre = 0; // 预圆环延时计数
static uint16 count_delay_R_circ_in = 0; // 入环延时
static uint16 count_delay_R_circ_out = 0;
static uint16 count_delay_L_circ_pre = 0; // 预圆环延时计数
static uint16 count_delay_L_circ_in = 0; // 入环延时
static uint16 count_delay_L_circ_out=0;
static uint16 count = 0;

float inc_L = 0.0;
float inc_R = 0.0;
float temp_L = 0.0;
float temp_R = 0.0;
float temp = 0;
float dif_rate = 0; // 正常行驶差速系数
float speed_rate=0;
float Sum_L, Sum_R, Sum_LM, Sum_RM, Sum_M= 0;

P42 = 1;
flag_trackout = 0;

count_led++;
if(count_led >= 0.5 * TIM1_ISR_F)
{
count_led = 0;
LED_toggle; // 用于调试，防止中断崩
}
// 速度
temp_L = ctimer_count_read(Encoder_L) * Pulse2MPM; // 左右轮当前速度

ctimer_count_clean(Encoder_L); // 编码器清零
ctimer_count_clean(Encoder_R);

if(DIR_Encoder_L == 1)
pid_motor_L.ActualValue = temp_L;
else
pid_motor_L.ActualValue = (-1) * temp_L;
if(DIR_Encoder_R == 0)
pid_motor_R.ActualValue = temp_R;
else
pid_motor_R.ActualValue = (-1) * temp_R;

// sample

// get sum
Sum_L  = L[0] + L[1] + L[2] + L[3] + L[4];
Sum_R  = R[0] + R[1] + R[2] + R[3] + R[4];
Sum_M  = M[0] + M[1] + M[2] + M[3] + M[4];

// normalize and mean filter

if(!test)
{
error1 = (1.0 / Lres - 1.0 / Mres) / (1.0 / Lres + 1.0 / Mres);
error2 = (1.0 / Mres - 1.0 / Rres) / (1.0 / Mres + 1.0 / Rres);
error = (error1 + error2) / (error1 + error2);

pid_steer.KP = 20 + fabs(error)*fabs(error) *10;

if(pid_steer.KP > 200)
pid_steer.KP = 200;

dif_rate = 0.001 + fabs(error) / 1000;
if(dif_rate >= 0.003)
dif_rate = 0.003;

speed_rate = 0.9 + 0.1 / fabs(error) / fabs(error);
if(speed_rate > 1.1)
speed_rate = 1.1;

pid_steer.ActualValue = error;
temp = PID_Control_Pos(&pid_steer, 1);

if((!flag_R_circ_in) && (!flag_L_circ_in) && (!flag_fork))
duty_steer = Middle + 1.0 * (uint16)(temp);

if(duty_steer > Left) // 舵机限幅
duty_steer = Left;
if(duty_steer < Right) // 舵机限幅
duty_steer = Right;

// 速度差速控制
if(Debug)
dif_rate = 0;

pid_motor_L.SetValue = speed_rate*speed_goal * (1 - dif_rate * temp) ; // 差速
pid_motor_R.SetValue = speed_rate*speed_goal * (1 + dif_rate * temp) ;

inc_L = PID_Control_Inc(&pid_motor_L, 1);
inc_R = PID_Control_Inc(&pid_motor_R, 1);

duty_L += inc_L;
duty_R += inc_R;

if(duty_L > Motor_UpperLimit)
duty_L = Motor_UpperLimit;
if(duty_L < Motor_LowerLimit)
duty_L = Motor_LowerLimit;

if(duty_R > Motor_UpperLimit)
duty_R = Motor_UpperLimit;
if(duty_R < Motor_LowerLimit)
duty_R = Motor_LowerLimit;

// out of track
if((Rres < 80 && Lres < 80) && (!Debug))
{

DIR_1 = 1;
pwm_duty(Motor_L, 0);
DIR_2 = 1;
pwm_duty(Motor_R, 0);
flag_trackout = 1;
}
else
{
pwm_duty(PWMB_CH1_P74, duty_steer);
DIR_1 = 0;
pwm_duty(Motor_L, duty_L);
DIR_2 = 0;
pwm_duty(Motor_R, duty_R);
}
}
P42 = 0;


THE END

)">