飞思卡尔光电直立

NXP

飞思卡尔光电直立

2019-07-12 13:39发布生成海报

站内文章 / NXP MCU

13173 0

直立平衡车包括，直立还，速度环，和CCD循迹设计。定时器产生中断，第1毫秒进行加速剂陀螺仪AD采样，第2毫秒进行角度融合，PWM控制输出，第3毫秒进行直立速度融合，第4ms进行CCD循迹方向控制。
/***************************************
Copyright:Electronic innovation laboratory
Author:niub
Date:2016-12-21
Version:V1.2
Description: the software of Freescale Carle photoelectric vertical design ****************************************/

#include "includes.h"
#include "math.h"
#include "HAL_GPIO.h"

#define STATIC_ADC_RESULT_X 0//390
#define STATIC_ADC_RESULT_Y 0//408
//函数声明
void init_gpio(void);//小灯初始化
void init_offset(); //开机初始化
void bianmaqi(void);//编码器函数声明
void SpeedControl(void);//速度控制
void SpeedControlOutput(void);//速度平滑输出
void DirectionControl();//方向控制
void DirectionControlOutput();//方向控制平滑输出
void MotorOutput() ;//电机输出函数声明
 extern uint8 IntegrationTime ;        //曝光时间
 extern void StartIntegration(void);     //曝光函数
/*********************************************************************************/
 void delay(void);
int16 adc_get_ave(ADCx, uint8, uint16);//函数声明
int16 adc_result_to_velocity(int16);//函数声明
float pwm1,pwm2;
float P;
float d;
/*********************CCD的参数定义********************************/
uint8 AtemP ;
uint8 Pixel[128];
uint8 send_data_cnt = 0;
uint8 *pixel_pt;
volatile uint8 k;
uint8 TIME0flag_5ms;//时间标志位
uint8 TIME0flag_10ms;
uint8 TIME0flag_15ms;
uint8 TIME0flag_20ms;
uint8 TIME1flag_20ms;
uint8 TIME1flag_1ms;
extern uint8 TimerFlag20ms;
/* 曝光时间，单位ms */
//unsigned char IntegrationTime = 10;
/******************************************************/
 int count2=0;  //不同中断片段

void  main(void)
{
  sysinit ();//时钟初始化
  init_gpio();//IO口初始化
  LPLD_ADC_Init(ADC0, MODE_10, CONV_SING);
  void LCD_Init(void);//液晶5110初始化
  delay();
  LPLD_PIT_Init(PIT0, 1000, pit_isr0);//进入中断

  LPLD_FTM0_PWM_Init(10000);//pwm初始化
  LPLD_FTM1_PWM_Init(100);//此通道专用于舵机控制
  LPLD_FTM1_PWM_Open(0,0);//PTA8
  LPLD_FTM0_PWM_Open(0,0);//PTC1  
  LPLD_FTM0_PWM_Open(1,0);//PTC2  
  LPLD_FTM0_PWM_Open(2,0);//PTC3
  LPLD_FTM0_PWM_Open(3,0);//PTC4 
  FTM1_QUAD_Iint(); //正交解码初始化  用于编码器
  FTM2_QUAD_Iint(); //正交解码初始化  用于编码器
CCD_init1();//CCD初始化
 pixel_pt = Pixel;//指针指向数组，将数组全部清零
  for(k=0; k<128+10; k++) 
  {
    *pixel_pt++ = 0;
  }

  while(1)
  {
      /**************************CCD程序**发送给上位机***************************************************************/  
     if(TIME1flag_20ms == 1)
      {
        TIME1flag_20ms = 0 ; 
        /* Sampling CCD data */
      ImageCapture(Pixel);
        /* Calculate Integration Time */
      CalculateIntegrationTime();
      if(++send_data_cnt >= 5) 
       {
         send_data_cnt = 0;
       SendImageData(Pixel);
        }
        /**********
        计算黑线位置
        **********/
           CCD_handle(); //CCD处理。。。

       Delay_us(100000); // Delay_us(100000);可以显示 有点闪

      LCD_clear();  
     Delay_us(100000); // Delay_us(100000);可以显示  有点闪
     LCD_write_english_string(24,0,"LANDZO");  
     LCD_write_english_string(24,1,"tangwenkai"); 

    //LCD_Write_Num(0,0,a,4);
    //LCD_Write_Num(0,1,b,4);
    //LCD_Write_Num(0,2,s2,4); 

      printf("mid_line[2]=%d
",mid_line[2]);
      printf("mid_line[3]=%d
",mid_line[3]);
      printf("Left=%d
",Left_point);
      printf("Right=%d
",Right_point);
     }                
  }    
 }    

/*******************************************进入中断处理程序***********************************************/
  void pit_isr0()
 {
    static int event_count; //不同中断片段
        g_nSpeedControlPeriod ++;
    SpeedControlOutput();//速度平滑控制输出
    g_nDirectionControlPeriod ++;
    DirectionControlOutput();//方向控制平滑输出

  /***************************************线性CCD自适应曝光******************************/

  static unsigned char TimerCnt20ms = 0;
   static unsigned char TimerCnt10ms =0;
  uint8 integration_piont;
  TIME1flag_1ms = 1 ;
  TimerCnt20ms++;
  TimerCnt10ms++;
  count2++;

 /* 根据曝光时间计算20ms周期内的曝光点*/
  integration_piont = 20- IntegrationTime; //20
  if(integration_piont >= 2) {      /* 曝光时间小于2则不进行再曝光 */
      if(integration_piont == TimerCnt20ms)
     StartIntegration();          ///曝光开始
  }

  if(TimerCnt20ms >= 20) {  
    TimerCnt20ms = 0;
    TIME1flag_20ms = 1;      
  }

  if(event_count >= 5)  //电极脉冲计数器读取与清除
    {
        event_count = 0 ;
                bianmaqi(); //编码器读取脉冲

    }
        else if(event_count == 1)   //启动AD转换 20次模拟量采集取平均值
    { 
        Adc0_Result=adc_get_ave(ADC0, 10, 20);        //取得PTA7引脚AD值  加速计               
            Adc1_Result=adc_get_ave(ADC0, 9, 20);         //取得PTB1引脚AD值  陀螺仪
            Adc2_Result=LPLD_ADC_SE_Get(ADC0, 12);//20);         //取得PTB2引脚AD值  陀螺仪//用于转向陀螺仪
        }
        else if(event_count == 2)                        //车模直立控制 (角度计算、直立控制计算、电机PWM输出)
        {

//          float fDeltaValue;  
//          angle_acc=(Adc0_Result-461)*1.31;//477//471//467 //457//459//481//476 //427             //411//410//0.2894;//0.2894;//465//464//458//450//0.2941;    
//          if(angle_acc>90) angle_acc=90;                 //防止加速度计角度溢出
//          if(angle_acc<-90)angle_acc=-90; 
//         
//          vr=(Adc1_Result-513)*-1.8;//519//517//515 //510               //      507    //514//508//503//504//510//504//470//478//476//471//487//568 //570//590//622//590  //626                   //610//616//610//605//600//615//602//539//588  //585          //-2           
//          angle1=angle_gyro;                                                         
//          fDeltaValue=(angle_acc-angle1)/1;          //0.035   //0.03  //0.05   //0.008   //0.015         
//          
//          angle_gyro+=(vr+fDeltaValue)/200;             //0.005       
//          
//         
//         
//         
//          
//          P=1450;  //1780...   //    1380    1200;//1200   //900   //130                                             
//          d=15;       //  //17  //10.58 //9.95//9.8     //25      //5.45   //10.85    //18.85    //22.95  //21.95   
//        
//         
//         
//
//                         //17   //10.58 //9.95//9.8   //25      //5.45   //10.85    //18.85    //22.95  //21.95   
//        
//         
//          pwm1=P*angle1+d*vr;
//          MotorOutput();//电机输出
//          
//          SDS_OutData[0]=(Adc0_Result-STATIC_ADC_RESULT_X);
//          SDS_OutData[1]=(Adc1_Result-STATIC_ADC_RESULT_X);//Adc0_Result
//          SDS_OutData[2]=(angle1-STATIC_ADC_RESULT_X);
//          SDS_OutData[3]=( Adc2_Result-STATIC_ADC_RESULT_X);//Adc1_Result
//     //   SDS_OutPut_Data(&SDS_OutData[0]); 
//          

        }


     else if(event_count == 3 )  //车模速度控制(PID调节，每秒调节10次)
     {  
//      static int SpeedControlCount; //0~19计数,在0片段进行PID调节
//      SpeedControlCount ++;
//      
//          if(SpeedControlCount >= 20) 
//                {
//                 SpeedControl();
//               
//              SpeedControlCount = 0;
//              g_nSpeedControlPeriod = 0;              
//          }
//      
      }




      else if(event_count == 4) //车模方向控制
     {
//      static int DirectionControlCount;
//              DirectionControlCount ++;
//       //aocao();//用于CCD
//           
//              
//               
//        
//      
//      if(DirectionControlCount >= 2) 
//                {
//          DirectionControl();
//          DirectionControlCount = 0;
//          g_nDirectionControlPeriod = 0;
//      }

     }   
    event_count ++;

 }  




/*******************************电机输出函数*********************************************************/
  void MotorOutput(void)  
   {



        g_fLeftMotorOut  = pwm1- g_fSpeedControlOut+ g_fDirectionControlOut ;//直立速度和方向
        g_fRightMotorOut = pwm1- g_fSpeedControlOut- g_fDirectionControlOut;


        if(g_fLeftMotorOut>6000)g_fLeftMotorOut=6000;
        if(g_fLeftMotorOut<-6000)g_fLeftMotorOut=-6000;
        if(g_fRightMotorOut>6000) g_fRightMotorOut=6000;

        if(g_fRightMotorOut<-6000) g_fRightMotorOut=-6000;


    if(g_fLeftMotorOut>=0)              //左轮速度大于零 向前的话

          {

         LPLD_FTM0_PWM_ChangeDuty(0,(int)(g_fLeftMotorOut));           //0左轮向前
         LPLD_FTM0_PWM_ChangeDuty(1,0);



          }
         else if(g_fLeftMotorOut<0)
         {

         LPLD_FTM0_PWM_ChangeDuty(1,(int)(-g_fLeftMotorOut));   //1左轮向后


         LPLD_FTM0_PWM_ChangeDuty(0,0);


         }
        if(g_fRightMotorOut>=0)
        {
          LPLD_FTM0_PWM_ChangeDuty(2,(int)(g_fRightMotorOut));   //2右轮向前
          LPLD_FTM0_PWM_ChangeDuty(3,0);
        }
        else if(g_fRightMotorOut<0)
        {
          LPLD_FTM0_PWM_ChangeDuty(3,(int)(-g_fRightMotorOut));  //3右轮向后
          LPLD_FTM0_PWM_ChangeDuty(2,0); 
        }
}

  void init_gpio()
{
  // 配置 PTD8~PTD15 为GPIO功能,方向为输出
  LPLD_GPIO_Init(PTD, 15, DIR_OUTPUT, OUTPUT_L, IRQC_DIS);
  LPLD_GPIO_Init(PTC, 0,  DIR_OUTPUT, OUTPUT_H, IRQC_DIS);
  LPLD_GPIO_Init(PTA, 17, DIR_OUTPUT, OUTPUT_H, IRQC_DIS);
  LPLD_GPIO_Init(PTE, 26, DIR_OUTPUT, OUTPUT_H, IRQC_DIS); 
}
//延时一段时间
void delay()
{
  unsigned int i, n;
  for(i=0;i<10000;i++)
  {
    for(n=0;n<800;n++)
    {
      asm("nop");
    }
  }
}

//取得平均AD值
int16 adc_get_ave(ADCx adc, uint8 channel, uint16 cnt)
{
  uint32 tmp=0;
  uint8 i;
  for(i=cnt; i>0; i--)
  {
    tmp += LPLD_ADC_SE_Get(adc, channel);

  }
  tmp /= cnt;
  return (int16)tmp;  
}

/*下面是CCD循迹代码，移植上届光电学长的。CCD是线阵，其本质就是根据采集到的黑白像素点的电压高低，来判断路径。为了确保准确存中点数据存了3次
，每18ms更新一次。*/

//part of CCD design #include “includes.h” /********************“CCD定义*****************************************/
int Mid_line_average;
float CCD_center =64;
unsigned char Mid_line=64; //CCD提取中线
unsigned char Left_Flag,Right_Flag;
unsigned char left_flag[3]={0,0,0};
unsigned char right_flag[3]={0,0,0};
unsigned char mid_line[4] = { 0,0,0,0};
unsigned char PRE_left_point[4]={0,0,0,0};
unsigned char PRE_right_point[4]={0,0,0,0};
//unsigned char pre_Mid_line;
//unsigned int Mid_line_sum;
unsigned char farlength=0;//定义白 {MOD}区域的宽度，初始时为零 unsigned char Right_point;
unsigned char Left_point; unsigned char Left_num = 74;//这两个值可以看情况而定
unsigned char Right_num = 54; unsigned char pre_Right_point;
unsigned char pre_Left_point; unsigned int Threshold_Value; //赛道判别
unsigned char startline_Flag=0;
unsigned int stop_Flag=0;
unsigned char white=0;
unsigned char black=0; int Average=0;
unsigned int MIN_pixel,MAX_pixel; unsigned char pixel3_pt[128]; // 取阀值转换用 unsigned char pixel_pt2[128]; unsigned int Flag_Threshold = 0; //取阀值 unsigned char data_pixel[128]; /* 128个像素点的平均AD值 */
unsigned char PixelAverageValue; /* 128个像素点的平均电压值的10倍 */
unsigned char PixelAverageVoltage; uint8 CCDCompleteFlag=0; /***************************************************************
* 蓝宙电子工作室
*
* 函数名称：CCD_init
* 功能说明：CCD初始化
* 参数说明：
* 函数返回：无
* 修改时间：2012-10-20
* 备注：
***************************************************************/
void CCD_init1(void)
{
LPLD_GPIO_Init(PTE, 0, DIR_OUTPUT, OUTPUT_H, IRQC_DIS); //gpio_init (PORTE , 4, GPO,HIGH);
LPLD_GPIO_Init(PTE, 2, DIR_OUTPUT, OUTPUT_H, IRQC_DIS); //gpio_init (PORTE , 5, GPO,HIGH);
LPLD_ADC_Init(ADC1, MODE_8, CONV_SING);//ADC1采集数据初始化。。。// adc_init(ADC1, AD6b) ; } /***************************************************************
* 蓝宙电子工作室
*
* 函数名称：StartIntegration
* 功能说明：CCD启动程序
* 参数说明：
* 函数返回：无
* 修改时间：2012-10-20
* 备注：
***************************************************************/
void StartIntegration(void) {

unsigned char i;

SI_SetVal();            /* SI  = 1 */
SamplingDelay();
CLK_SetVal();           /* CLK = 1 */
SamplingDelay();
SI_ClrVal();            /* SI  = 0 */
SamplingDelay();
CLK_ClrVal();           /* CLK = 0 */

for(i=0; i<127; i++) {
    SamplingDelay();
    SamplingDelay();
    CLK_SetVal();       /* CLK = 1 */
    SamplingDelay();
    SamplingDelay();
    CLK_ClrVal();       /* CLK = 0 */
}
SamplingDelay();
SamplingDelay();
CLK_SetVal();           /* CLK = 1 */
SamplingDelay();
SamplingDelay();
CLK_ClrVal();           /* CLK = 0 */

} /***************************************************************
* 蓝宙电子工作室
*
* 函数名称：ImageCapture
* 功能说明：CCD采样程序
* 参数说明：* ImageData 采样数组
* 函数返回：无
* 修改时间：2012-10-20
* 备注：
*ImageData = ad_once(ADC1, AD6a, ADC_8bit);
***************************************************************/ void ImageCapture(unsigned char * ImageData) {

unsigned char i;
extern uint8 AtemP ;

SI_SetVal();            /* SI  = 1 */
SamplingDelay();
CLK_SetVal();           /* CLK = 1 */
SamplingDelay();
SI_ClrVal();            /* SI  = 0 */
SamplingDelay();

`这里写代码片`for(i = 0; i < 250; i++) {                    //更改250，让CCD的图像看上去比较平滑，
  SamplingDelay() ;  //200ns                  //把该值改大或者改小达到自己满意的结果。
}

*ImageData = LPLD_ADC_SE_Get(ADC1 , 5); //取得PTE1引脚的AD值。。//ad_once(ADC1, AD6b, ADC_8bit);
ImageData ++ ;
CLK_ClrVal();           /* CLK = 0 */

for(i=0; i<127; i++) {
    SamplingDelay();
    SamplingDelay();
    CLK_SetVal();       /* CLK = 1 */
    SamplingDelay();
    SamplingDelay();

   *ImageData =LPLD_ADC_SE_Get(ADC1,5);//  ad_once(ADC1, AD6b, ADC_8bit);
    ImageData ++ ;
    CLK_ClrVal();       /* CLK = 0 */
}

SamplingDelay();
SamplingDelay();
CLK_SetVal();           /* CLK = 1 */
SamplingDelay();
SamplingDelay();
CLK_ClrVal();           /* CLK = 0 */

} /***************************************************************
* 蓝宙电子工作室
*
* 函数名称：CalculateIntegrationTime
* 功能说明：计算曝光时间
* 参数说明：
* 函数返回：无
* 修改时间：2012-10-20
* 备注：
***************************************************************/ /* 曝光时间，单位ms */
uint8 IntegrationTime = 10; /* 128个像素点的平均电压值的10倍 */
uint8 PixelAverageVoltage; /* 设定目标平均电压值，实际电压的10倍 */
int16 TargetPixelAverageVoltage = 25; /* 设定目标平均电压值与实际值的偏差，实际电压的10倍 */
int16 PixelAverageVoltageError = 0; /* 设定目标平均电压值允许的偏差，实际电压的10倍 */
int16 TargetPixelAverageVoltageAllowError = 2; void CalculateIntegrationTime(void) {

/* 计算128个像素点的平均AD值 */

PixelAverageValue = PixelAverage(128,Pixel);

/* 计算128个像素点的平均电压值,实际值的10倍 */
PixelAverageVoltage = (unsigned char)((int)PixelAverageValue * 25 / 194);

PixelAverageVoltageError = TargetPixelAverageVoltage - PixelAverageVoltage;
if(PixelAverageVoltageError < -TargetPixelAverageVoltageAllowError)
{
  PixelAverageVoltageError = 0- PixelAverageVoltageError ;
  PixelAverageVoltageError /= 2;
  if(PixelAverageVoltageError > 10 )
     PixelAverageVoltageError = 10 ;
   IntegrationTime -= PixelAverageVoltageError;
}
if(PixelAverageVoltageError > TargetPixelAverageVoltageAllowError)
{ 
    PixelAverageVoltageError /= 2;
    if(PixelAverageVoltageError > 10 )
       PixelAverageVoltageError = 10 ;
    IntegrationTime += PixelAverageVoltageError;
}

if(IntegrationTime <= 1)
    IntegrationTime = 1;
if(IntegrationTime >= 100)
    IntegrationTime = 100;

}

飞思卡尔光电直立

Ta的文章更多 >>

热门文章

飞思卡尔光电直立

Ta的文章 更多 >>

热门文章

举报内容

检举类型

检举原因

检举说明(必填)

打开微信“扫一扫”，打开网页后点击屏幕右上角分享按钮

Ta的文章更多 >>