NXP

NXP 节能组程序

2019-07-12 11:30发布

站内文章 / NXP MCU
14496 0 1674
#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; }