KEA128电磁智能车程序

ID:388667 · 发表于 2018-8-21 20:12

#include "headfile.h"
#include "KEA128_uart.h"
#include "KEA128_qiu.h"
#include "filter.h"
#include "All_Init.h"
#define Speed_set 0
#define zhong 2250
struct PID_Typedef //PID结构体
{
float P;
float I;
float D;
int Error;
int PreError;
int PreDError;//
int d_error;
int dd_error;
int Output;
int target;
int measure;
}PID_Typedef;
float Kp,Kd,Ks;
extern uint16 val_get_L;//左侧电感电压值 PTA8
extern uint16 val_get_R;//右侧电感电压值 PTA7
extern uint16 aveL,aveR;
extern float val_dev_to_one,val_sum_to_one,aveL_to_one,aveR_to_one;
//extern uint16 val_get_M;
extern uint8 time_flag,dir_time;
extern uint8 read_bianmaqi;
uint8 * itoa(int32 num);
uint16 SpeedSumL,SpeedSumR;
uint16 SpeedL,SpeedR;
uint16 uart_cnt=0;
uint16 sum_cnt;
double Speed_L;
double Speed_R;
float ActualErr=0.0;
float Err_L;
float Err_N;
float Error=0.0;
void speedout();
//void differential();
void ReadBianMaQi();
void Ad_deal();
void MOTOR_PID(struct PID_Typedef *PID,int target,int measure);
void PID_init();//pid参数赋值
struct PID_Typedef Left;
struct PID_Typedef Right;
int main(void)
{
get_clk(); //获取时钟频率必须执行
ALL_Init();
PID_init();
pit_init_ms(pit0,1); //初始化pit0 周期设置为10ms
set_irq_priority(PIT_CH0_IRQn,1); //设置pit0优先级
enable_irq(PIT_CH0_IRQn); //开启pit0中断
EnableInterrupts;
for(;;)
{
ReadBianMaQi();//读取脉冲10读取一次，
speedout(); //40ms进行一次控速
if(time_flag==50)
{
time_flag=0;
//uart_cnt++;
//if(uart_cnt>=10)
//{
//uart_cnt=0;
//}
}
}
}
void ReadBianMaQi()
{
if(read_bianmaqi>=10)
{//10ms读取一次编码器
read_bianmaqi=0;
adc_get();
filter1();
adc_to_one();
Ad_deal();
SpeedL = ftm_count_get(ftm1);//读取脉冲
ftm_count_clean(ftm1);//清除寄存器
SpeedR = ftm_count_get(ftm0);//读取脉冲1
ftm_count_clean(ftm0);//清楚寄存器
SpeedSumL +=SpeedL;
SpeedSumR +=SpeedR;
SpeedL=0;
SpeedR=0;
}
}
void speedout()
{
if(sum_cnt>=40)
{//40ms进行一次速度控制
sum_cnt=0;
Left.measure=SpeedSumL;
Right.measure=SpeedSumR;
SpeedSumL =0;
SpeedSumR =0;
// Left.target=200;
// Right.target=200;
if( ActualErr>0)//在一定的范围内才加差速
{
Right.target=Speed_set-ActualErr*Ks;
Left.target=Speed_set+ActualErr;
}
else
{
Left.target=Speed_set+ActualErr*Ks;
Right.target=Speed_set-ActualErr;
}
if(Left.target > 700) //限幅防止输出过大
Left.target= 700;
if(Left.target <-700) //限幅防止输出过大
Left.target=-700;
if(Right.target > 700)
Right.target= 700;
if(Right.target <-700)
Right.target=-700;
// MOTOR_PID(&Left,Left.target,Left.measure);
// MOTOR_PID(&Right,Right.target,Right.measure);
// Left.target=200;
// Right.target=200;
Left.Output=Left.target;
Right.Output=Right.target;
if(Left.Output>=900)//限上值但是在这里不管下限在计算里面管
Left.Output = 900;
else if(Left.Output<=-900)
Left.Output =-900;
if(Right.Output>=900)//限上值但是在这里不管下限在计算里面管
Right.Output = 900;
else if(Right.Output<=-900)
Right.Output =-900;
if(Left.Output>=0)
{
Left.Output=(uint16)Left.Output+0;
ftm_pwm_duty(ftm2,ftm_ch2,0);
ftm_pwm_duty(ftm2,ftm_ch3,Left.Output);
}
else
{
Left.Output=(uint16)(-Left.Output)+0;
ftm_pwm_duty(ftm2,ftm_ch3,0);
ftm_pwm_duty(ftm2,ftm_ch2,Left.Output);
}
if(Right.Output>=0)
{
Right.Output=(uint16)Right.Output+0;
ftm_pwm_duty(ftm2,ftm_ch0,0);
ftm_pwm_duty(ftm2,ftm_ch1,Right.Output);
}
else
{
Right.Output=(uint16)(-Right.Output)+0;
ftm_pwm_duty(ftm2,ftm_ch1,0);
ftm_pwm_duty(ftm2,ftm_ch0,Right.Output);
}
/*ftm_pwm_duty(ftm2,ftm_ch0,Left.Output); //，设置占空比，，右电机正转,,在系统配置中已经将PWMIO口改成C0C1C2C3
ftm_pwm_duty(ftm2,ftm_ch1,0);
ftm_pwm_duty(ftm2,ftm_ch2,0); //左电机正转
ftm_pwm_duty(ftm2,ftm_ch3,Right.Output); */
uart_cnt++;
if(uart_cnt>=25)
{//4*25=1000 1秒输出一次上位机，方便观察
uart_cnt=0;
uart_putstr(uart2,"L编码器目标");
uart_putstr(uart2,itoa(Left.target));
uart_putstr(uart2,"\t");
uart_putstr(uart2,"R编码器目标");
uart_putstr(uart2,itoa(Right.target));
uart_putstr(uart2,"\n");
uart_putstr(uart2,"L编码器速度");
uart_putstr(uart2,itoa(Left.measure));
uart_putstr(uart2,"\t");
uart_putstr(uart2,"R编码器速度");
uart_putstr(uart2,itoa(Right.measure));
uart_putstr(uart2,"\n");
uart_putstr(uart2,"左边感量");
uart_putstr(uart2,itoa(aveL));
uart_putstr(uart2,"\t");
uart_putstr(uart2,"右边感量");
uart_putstr(uart2,itoa(aveR));
uart_putstr(uart2,"\n");
uart_putstr(uart2,"LDuty");
uart_putstr(uart2,itoa(Left.Output));
uart_putstr(uart2,"\t\t");
uart_putstr(uart2,"RDuty");
uart_putstr(uart2,itoa(Right.Output));
uart_putstr(uart2,"\n");
}
}
}
//整数转换为字符串（输入整数，返回字符串地址）
uint8 * itoa(int32 num)
{
int32 i=0,isNegative=0;
static uint8 s[15]; //必须是全局变量或者是静态变量
//如果是负数，先转为正数
if((isNegative=num) < 0) num = -num;
//从个位开始变为字符，直到最高位，最后应该反转
do
{
s[i++] = num%10 + '0';
num /= 10;
}while(num > 0);
//如果是负数，补上负号
if(isNegative < 0) s[i++] = '-';
//最后加上字符串结束符
s[i] = '\0';
/******翻转字符串******/
//p指向s的头部
uint8 *p = s;
//q指向s的尾部
uint8 *q = s;
while(*q) ++q;
q--;
//交换移动指针，直到p和q交叉
uint8 temp;
while(q > p)
{
temp = *p;
*p++ = *q;
*q-- = temp;
}
return s;
}
/*pid 调电机*/
/*****差速函数******/
/*void differential()
{
int chasu=0;
errorL=aveL-zhong;
errorR=aveR-zhong;
tempL=errorL*100;
tempL=tempL/zhong;
tempR=errorR*100;
tempR=tempR/zhong;
Right.target=(int)(speed_set-error*speed_set/100);
Left.target=(int)(speed_set-error*speed_set/100);
*/
/* if( ActualErr>0)//在一定的范围内才加差速
{
Right.target=(int)speed_set+ ActualErr;
Left.target=(int)speed_set-ActualErr*Ks;
}
else
{
Right.target=(int)speed_set-ActualErr;
Left.target=(int)speed_set+ ActualErr*Ks;
}
if( Left.target> 700) //限幅防止输出过大
Left.target= 700;
if( Left.target <-700) //限幅防止输出过大
Left.target=-700;
if( Right.target > 700)
Right.target= 700;
if( Right.target <-700)
Right.target=-700;
//给各自的target赋值
}*/
/******电机PID部分程序**********/
/*增量式 pid ，慢慢调 */
void MOTOR_PID(struct PID_Typedef *PID,int target,int measure) //target目标期望值，measure目标测量值，输出占空比改变量
{
PID->Error=target - measure; //计算偏差。
PID->d_error = PID->Error - PID->PreError; //偏差计算(比例)
PID->dd_error = PID->d_error - PID-> PreDError; //偏差计算(微分)
PID->PreError = PID->Error; //存储当前偏差
PID->PreDError = PID->d_error;
if ((PID->Error > -5) && (PID->Error <5)) // 设置调节死区编码器返回的值跟目标值在小误差范围内不调节防止超调过度起反作用
{
; // 不执行PID调节
}
else
{
PID->Output+= ((PID->P* PID->d_error) + (PID->I * PID->Error) + (PID->D * PID->dd_error));
}
if(PID->Output>=900)//限上值但是在这里不管下限在计算里面管
PID->Output = 900;
else if(PID->Output<=0)
PID->Output =0;
}
void Ad_deal()
{
float IncrementErr;
Error=val_dev_to_one*100/pow(val_sum_to_one,1.5); //水平电感偏差
if(Error>=100)
Error=100;
if(Error<=-100)
Error=-100;
// Error=(sqrt(AD3)-sqrt(AD1))/(AD1+AD3)*3800;
/****************增量式PD****************/
IncrementErr=Kp*(Error-Err_N)+Kd*(Error-2*Err_N+Err_L); //增量式PD
ActualErr+=IncrementErr; //
Err_L=Err_N; //
Err_N=Error;
// Lost_Err(); //防丢线程序
/*****************两轮差速效果*****************/
if(Error<=0) //计算两轮差速效果
Error=-Error; //
Ks=Error/10; //
if(Ks>1.3) //
Ks=1.3; //0.78 //增大后内轮减速明显
}
void PID_init()
{
Left.P = 0.4; //左轮P
Left.I = 0.28; //左轮I
Left.D = 0; //
Right.P = 0.4; //右轮P
Right.I = 0.28; //右轮I
Right.D = 0; //右轮D
/* 电感差速 */
Error=0.0;
Err_N=0.0;
Err_L=0.0;
Kp=5.0;
Kd=20.0;
/*左右轮差速效果*/
Ks=0.0; //在arithmetic.c中赋值Error/10
}

复制代码

帐号		自动登录	找回密码
密码			立即注册