NXP 节能组程序
2019-07-12 11:30发布
生成海报
#include "common.h"
#include "include.h"
//关于电感
//adc经过判断和计算后的偏差
uint16 AD[8], AD_v[8];
uint16 max_v[8] = { 2516,2619,0,2573,2573,0,0,0 }, min_v[8] = { 39,50,50,42,50,50,50,50 }; //找到6个电感中的最大最小值，电感标定采集值
int16 cline_err;
uint16 adc_av(ADCn_Ch_e adcn_ch, ADC_nbit bit);
void Read_ADC(void);
void adc_max(void);
void to_one(void);
void adc_init1(void);
uint8 stop = 3;
uint8 ms = 0;
//关于编码器
//记录编码器采集的脉冲数
//设定速度
//pid计算后应该给予的速度电压
//当前速度获取程序 //编码器初始化
float djc;
uint8 count = 0;
uint16 cspeed;
uint16 speed;
uint16 uspeed;
uint16 speedh = 0;
uint32 angle;
uint32 lastangle;
void bmq_get(void);
//虚拟示波器程序
void vcan_sendware1(uint8 *wareaddr, uint32 waresize);
//以下为多种pid的运算方案
float Moto_PIDyou(float Target_Speed, float System_Feed_Speed);
float duoji_PID3(float cline_err); //变系数PD舵机控制(位置式) KD可变，
////////////////控制参数 ////////////////
int DuoP = 24; //
int DuoD = 2;
int timecount;
int speedcnt;
int speedkp;
int dianjispeed;
int errz[8];
int flag = 0;
unsigned char Dir_last = 0;
int dir_error = 0, dis_error;
int Calculate_Length;
int DirectionErr[9] = { 0 };
int disgy_AD_val_1, disgy_AD_val_2;
float Error_Delta;
unsigned short servPWMDuty; //当前舵机值
int8 flag1 = 0;
int8 flag2 = 0;
int8 flag3 = 0;
int8 flags = 0;
int8 diuxian = 0;
int8 zhidao = 0;
void Push_And_Pull(int *buff, int len, int newdata);
float Slope_Calculate(uint8 begin, uint8 end, int *p);
void adjduoji();
void adjspeed(uint16 speed, uint16 cspeed);
///////////////////////////以下是正式进入调车程序///////////////////////////////
//清除中断标志位,一定要清除，不然就一直在中断
void pit_ch1_irq(void)
{
ftm_pwm_duty(FTM2, FTM_CH5, 0);
float o_pwm;
Read_ADC();
adc_max();
to_one();
//AD[0]=AD[0];
djc = (float)(AD[4] - AD[0] - 3);
angle = duoji_PID3(djc) + 4800;
/*******正常道路*********/
if (abs(djc)<12)
uspeed = 300;
else if (25>abs(djc) >= 12)
uspeed = 150;
else
uspeed = 0;
while (flag1 == 0 && AD_v[3]>1450 && AD_v[1]<1150)
{
Read_ADC();
// adc_max();
to_one();
if (AD_v[3]>1450 && AD_v[1]<1150)
{
// djc=16*(float)(AD[3]-AD[1]);
angle = 5200;
ftm_pwm_duty(FTM2, FTM_CH4, angle);
flags = 1;
led(LED0, LED_ON);
}
else
{
djc = (float)(AD[4] - AD[0]);
ftm_pwm_duty(FTM2, FTM_CH4, angle);
led(LED0, LED_OFF);
// flags=2;
// flag1=1;
// led(LED0,LED_OFF);
// led (LED1,LED_ON);
}
if (70>AD_v[0])
{
flags = 2;
flag1 = 1;
led(LED0, LED_OFF);
led(LED1, LED_ON);
ftm_pwm_duty(FTM2, FTM_CH4, 4800);
//
// djc=(float)(AD[4]-AD[0]+3);
// angle=duoji_PID3(djc)+4800;
// ftm_pwm_duty(FTM2,FTM_CH4,angle); //正常走
}
}
ftm_pwm_duty(FTM2, FTM_CH4, 4800);
while (flag2 == 0 && AD_v[1]>1450 && AD_v[3]<1250)
{
Read_ADC();
// adc_max();
to_one();
ftm_pwm_duty(FTM2, FTM_CH4, 4250);
if (AD_v[1]>1450 && AD_v[3]<1250)
{ //led(LED1,LED_OFF);
led(LED2, LED_ON);
// djc=16*(float)(AD[3]-AD[1]);
angle = 4250;
ftm_pwm_duty(FTM2, FTM_CH4, angle);
flags = 3;
}
else
{
djc = (AD[4] - AD[0]);
ftm_pwm_duty(FTM2, FTM_CH4, angle);
led(LED2, LED_OFF);
// led (LED3,LED_ON);
// flags=4;
// flag2=1;
}
if (110>AD_v[4])
{
led(LED2, LED_OFF);
led(LED3, LED_ON);
flags = 4;
flag2 = 1;
ftm_pwm_duty(FTM2, FTM_CH4, 4800);
// djc=(float)(AD[4]-AD[0]+3);
// angle=duoji_PID3(djc)+4800;
// ftm_pwm_duty(FTM2,FTM_CH4,angle); //正常走
}
}
ftm_pwm_duty(FTM2, FTM_CH4, 4800);
//led (LED3,LED_OFF);
flags = 0;
ftm_pwm_duty(FTM2, FTM_CH4, angle); //正常走
if (abs(djc)<10 && 400 AD_duo[i][k + 1]) //前面的比后面的大 则进行交换
{
temp = AD_duo[i][k + 1];
AD_duo[i][k + 1] = AD_duo[i][k];
AD_duo[i][k] = temp;
}
}
}
}
for (i = 0; i<8; i++) //求中间三项的和
{
ad_sum[i] = AD_duo[i][1] + AD_duo[i][2] + AD_duo[i][3];
AD_v[i] = ad_sum[i] / 3;
}
}
void adc_max(void)
{
uint8 j;
for (j = 0; j<8; j++)
{
if (AD_v[j] > max_v[j] + 20)
{
max_v[j] = AD_v[j];
}
if (AD_v[j]1.0) sensor_to_one[i] = 1.0;
AD[i] = (uint16)(100 * sensor_to_one[i]); //AD[i]为归一化后的值 范围为0-100
}
}
/**************************pid运算方案*************************************/
float duoji_PID3(float cline_err) //变系数PD舵机控制(位置式) KD可变，
{
volatile static float err = 0, last_err = 0, derr = 0, l_last_err, dderr;
volatile static float M_PWM = 0;
volatile static float Kd, Kp, Ki;
err = cline_err;
// Push_And_Pull(errz,8,err);
//derr=100*Slope_Calculate(0,8,errz);//偏差50代表中间
derr = err - last_err; //derr dderr赋初值
dderr = err - 2 * last_err + l_last_err;
/******DIU XIAN*******/
if (AD[0]>19 && AD[4]>19) //直道
{
if (err<0)
Kp = 1.8;
else if (err>0)
Kp = 1.6;
zhidao = 1;
diuxian = 0;
}
// if(abs(err)>35)
Kp = 2 + exp(0.025*abs(err));
if (flag1 == 1 || flag2 == 1)
Kp = 2 + exp(0.05*abs(err));
// if(abs(err)<800)
Kd = 110 - exp(0.045*abs(err));
// else Kd++;
Ki = 0;
M_PWM = err * Kp + derr * Kd + Ki * dderr;; //注意这里是非增量式PID
// }
l_last_err = last_err; //记录上上次偏差last_err-err;/7记录上次偏差值servo_control (M_PWM);最后赋值给舵机函数
last_err = err;
/*************XDIU IAN***********/
if (AD_v[0]<100 && AD[4]<100 || AD_v[0]<80 || AD_v[4]<80)
{
if (AD[0]AD[4])
M_PWM = -500;
diuxian = 1;
zhidao = 0;
}
/*************XIAN WEI***********/
if (M_PWM>450)
M_PWM = 450;
if (M_PWM<-510)
M_PWM = -510;
return M_PWM;
}
float Moto_PIDyou(float Target_Speed, float System_Feed_Speed) //电机的pid函数
{
static float err = 0, last_err = 0, derr = 0;
static long int M_PWM = 0;
static float Kd;
float Kp;
float temp;
err = Target_Speed - System_Feed_Speed;
derr = err - last_err;
if (fabs(err)>10)
Kp = 200;
else
Kp = 10;
if (fabs(err)<10)
Kd = 600;
else
Kd++;
temp = err * Kp + derr * Kd;
M_PWM += temp;
last_err = err;
if (M_PWM>9500)
M_PWM = 9500;
if (M_PWM<-9500)
M_PWM = -9500;
return(M_PWM); //返回PWM的值
}
/////////////////最小二乘法拟合斜率，求电感插值导数/////////////////
float Slope_Calculate(uint8 begin, uint8 end, int *p)
{
int xsum = 0, ysum = 0, xysum = 0, x2sum = 0;
uint8 i = 0;
float result = 0;
static float resultlast;
p = p + begin;
for (i = begin; i0; i--)
{
*(buff + i) = *(buff + i - 1);
}
*buff = newdata;
}
打开微信“扫一扫”，打开网页后点击屏幕右上角分享按钮
{var%20f='http://v.t.sina.com.cn/share/share.php?appkey=1515056452',u=z||d.location,p=['&url=',e(u),'&title=',e(t||d.title),'&source=',e(r),'&sourceUrl=',e(l),'&content=',c||'gb2312','&pic=',e(p||'')].join('');function%20a(){if(!window.open([f,p].join(''),'mb',['toolbar=0,status=0,resizable=1,width=440,height=430,left=',(s.width-440)/2,',top=',(s.height-430)/2].join('')))u.href=[f,p].join('');};if(/Firefox/.test(navigator.userAgent))setTimeout(a,0);else%20a();})(screen,document,encodeURIComponent,'','','https://www.xiaopingtou.cn//data/attach/topic/topicKPo7gB.jpg', '推荐 XinChao 的文章《NXP 节能组程序》','https://www.xiaopingtou.net/article-64096.html','页面编码gb2312|utf-8默认gb2312'));){kind=link}