设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 电子技术分类讨论区 › 智能小车/机器人 › K60_CCD直立小车源码(二等奖程序)
返回列表 发新帖
查看: 3061|回复: 1
打印 上一主题 下一主题
收起左侧

K60_CCD直立小车源码(二等奖程序)

[复制链接]
跳转到指定楼层
楼主
ID:327139 发表于 2018-5-10 21:07 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
k60 ccd二等奖程序

本程序配套4按键直立一体板

按键功能：
E0： 参数加
E1： 参数减
E2： 切换显示及功能
E3： 回到最初显示及功能


调试直立

A、修改直立静态偏移量： calcultion.c   s32 GRAVITY_OFFSET=485;//230  MPU6550为平衡时Z轴角度，放大10倍
B、main.c里 run()函数中：  修改  PWMout=MotorSpeedOut(AAngPWM,MotorSpeedPWM,MotorTurnPWM);//电机输出
                            为： PWMout=MotorSpeedOut(AAngPWM,0,0);//关闭速度环和转向环分量，直立后再加上两个分量



系统连线说明 ：


单片机源程序如下:
  1. #include "include.h"
  2. #include "calculation.h"
  3. #include"VisualScope.h"
  4. #include "CCD.h"
  5. #include "LQ12864.h"
  6. #include "MPU6050.h"

  8. int16 angletmp,angle_g,angle_w;  //16位
  9. int16 angle6550;  //MPU6550计算

  11. extern byte F6x8[][6];
  12. extern s16 g_nLeftMotorPulseSigma,g_nRightMotorPulseSigma;
  13. extern float GYROSCOPE_ANGLE_RATIO;
  14. s32 TurnPosition;//实际位置
  15. s32 TurnPositionOld; //上一次位置
  16. int dline; //左右边界差

  18. float  ANGLE_CONTROL_P=240;//* 260    //直立P  260
  19. float  ANGLE_CONTROL_D=30;//1.5;//*  4.8   //直立D  5.8

  21. int16 xgout=1000;   //MPU6050六轴参数  x轴角速度
  22. int16 ygout=1000;
  23. int16 zgout=1000;
  24. int16 xaout=-1000;   //x轴加速度
  25. int16 yaout=-1000;
  26. int16 zaout=-1000;

  28. uint8 PWMDEADL = 0;  //死区电压左路电机
  29. uint8 PWMDEADR = 0;  //死区电压右路电机

  31. uint16 MAXTURNPWM= 200;  //电机转向最大PWM值

  33. #define   SAVEADRESS   (0X1C)

  35. extern s32 GRAVITY_OFFSET;
  36. uint8 disen;  //允许上传CCD图像

  38. uint16 A_P,A_D,V_P,V_I,T_P,T_D;  //从FLASH中读出的参数
  39. uint32 G_RATIO;
  40. uint8 flashreadbuf[6];

  42. u16 SpeedKP = 24;//4.4;//*              //速度P 放大10倍
  43. u16 SpeedKI = 6;//*               //速度D 放大10倍

  45. u16 TurnP1 = 80;  //直道前进时舵机PD值，放大10倍
  46. u16 TurnD1 = 2;

  48. //u16 TurnP2 = 34;//70;  //弯道时舵机转角比例系数 放大10倍
  49. //u16 TurnD2 = 140;//30;  //弯道时舵机转角积分系数  放大100倍
  50. //
  51. //u16 TurnP3 = 40;  //大弯时转角比例
  52. //u16 TurnD3 = 110;   //大弯时转角积分
  53. //
  54. //u16 TurnP4 = 48;  //大弯时转角比例
  55. //u16 TurnD4 = 90;   //大弯时转角积分

  57. u16 TurnP;     //* 94                 //方向P
  58. u16 TurnD;   //* 8.4              //方向D
  59. u16 turnsetmax;  //转向最大PWM值
  60. u16 turnpwmmax;
  61. u8  disccd[128][4];
  62. u8  paraadjflag;   //参数修改组切换


  65. extern int leftline, rightline; //左右边界值
  66. extern int leftlineold,rightlineold; //左右边界上一次值
  67. extern int yuzhizhi;//动态阈值
  68. extern signed int sumlspeed,sumrspeed;

  70. s16 SetSpeed= -250;
  71. s16 AmSpeed;//*   //目标速度 用于外部       设置速度时在这里设置
  72. s16 g_Speedgoal=0;//用于内部 自加速使用
  73. //时间标志位
  74. extern u8 TIME0flag_5ms,TIME0flag_10ms,TIME0flag_20ms;
  75. extern u8 TIME1flag_100ms,flag_1ms ;
  76. extern u8 TIME1flag_1s   ;  //PT1口1s定时标志位
  77. extern s16 whitelength;//91;   //具体看CCD图像，赛道宽度



  81. s32 ATimeCount=700;//100ms进入标志  由20个5ms构成，速度PID时使用
  82. s32 TimeCount=0 ;//1ms中断标志
  83. //角度传感器
  84. s32 GYROSCOPE_OFFSET,GYROSCOPE_turn_OFFSET;//陀螺仪静止时的零点
  85. s32 AD_ACC_Z;//加速度计的Z轴
  86. s32 AD_GYRO;//平衡陀螺仪
  87. s32 AD_GYRO_turn;//转向陀螺仪
  88. s32 AAngPWM=0,LastAAngPWM=0,AAngPeriodCount=0,MotorAAngPWM=0 ;
  89. //速度变量
  90. float g_SpeedControlOutNew,g_SpeedControlOutOld ;
  91. s16 SpeedPeriodCount=0;
  92. s32 MotorSpeedPWM=0 ;
  93. s32 PWMout ;
  94. int start_flag=0,stop_jiasu=0;
  95. //CCD变量
  96. int ccd_count=0;
  97. uint8_t Pixel[128];
  98. int ccd_flag=0;
  99. s16 TurnPeriodCount=0 ;
  100. s32 MotorTurnPWM=0 ;
  101. s16 TurnPWMOUT=0 ;
  102. s16 LastTurnPWMOUT=0;
  103. u16 discnt;
  104. u8  workflag;


  107. void run();//直立主函数
  108. void qibu();//起步不能一下把速度加上去，速度要慢慢加
  109. void GPIO_Init();
  110. void dispage1();
  111. void dispage2();
  112. void dispage3();
  113. void disdata1();
  114. void disdata2();
  115. void disdata3();
  116. void disccdval();
  117. extern void Pause(void);
  118. extern void LCD_CLS(void);

  120. void main()
  121. {  int16 ii;
  122.    discnt=0;
  123.    paraadjflag=0;
  124.    workflag=0;
  125.    AmSpeed = SetSpeed;
  126.    turnsetmax = 650;
  127.    s32 xgtemp,ygtemp;
  128.    DisableInterrupts; //禁止总中断
  129.    uart_init (UART0,115200);//初始化UART0，输出脚PTA15，输入脚PTA14
  130.    //AngleAcceleration_init() ;//AD初始化
  131.    FTM1_QUAD_Iint();//正交解码测速  A相---PTA8  B相---PTA9
  132.    FTM2_QUAD_Iint();//正交解码测速  A相---PTA10 B相---PTA11
  133.    oled_init();//oled初始化
  134.    CCD_init (); //CCD初始化
  135.    GPIO_Init(); //程序运行灯
  136.    pit_init_ms(PIT0, 1);  //初始化PIT0，定时时间为： 1ms
  137.    pit_init_ms(PIT1, 100);//初始化PIT1，定时时间为： 100ms
  138.    FTM_init() ; //PWM初始化
  139.    delayms(100);
  140.    MPU6050_Init();  //MPU6050初始化  , PD8  , PD9
  141.    delayms(300);
  142.    xgtemp=0;
  143.    ygtemp=0;
  144.    TurnP=TurnP1;
  145.    TurnD=TurnD1;
  146.    for(ii=0;ii<100;ii++)
  147.    {
  148.      xgtemp += MPU6050_GetDoubleData(MPU6050_GYRO_XOUT);//读X轴角速度,直立
  149.      Pause();
  150.      ygtemp += MPU6050_GetDoubleData(MPU6050_GYRO_YOUT);//读Y轴角速度，转向
  151.      Pause();
  152.    }
  153.    GYROSCOPE_OFFSET = xgtemp/100;        //静态X轴角速度值，直立，理论上为0，但实际可能不为0
  154.    GYROSCOPE_turn_OFFSET = ygtemp/100;   //静态Y轴角速度值，转向，理论上为0，但实际可能不为0
  155.    
  156.    EnableInterrupts;//开总中断  
  157.    uart_irq_EN(UART0);
  158.    
  159.    uart_putchar(UART0,'O');
  160.    uart_putchar(UART0,'K');

  162.    A_P = ANGLE_CONTROL_P;
  163.    A_D = ANGLE_CONTROL_D*10;
  164.    V_P = SpeedKP;
  165.    V_I = SpeedKI;
  166.    T_P = TurnP;
  167.    T_D = TurnD;
  168.    G_RATIO = GYROSCOPE_ANGLE_RATIO*100;
  169.    LCD_CLS();
  170.    dispage1(); //正常显示
  171.    while(1)
  172.    {
  173.      if(workflag==0)  //正常状态
  174.      { if(gpio_get(PORTE,3)==0)  //状态切换
  175.        { delayms(4);
  176.          if(gpio_get(PORTE,3)==0)
  177.          {
  178.            workflag=1;
  179.            LCD_CLS(); //清屏
  180.            dispage2();
  181.            while(gpio_get(PORTE,3)==0); //等待按键松开
  182.          }
  183.        }
  184.        discnt++;
  185.        if(discnt>=100)
  186.        { discnt=0;
  187.          disdata1();
  188.          delayms(2);
  189.        }
  190.        delayms(1);
  191.      }
  192.      else if(workflag==1)   //显示和上传CCD图像
  193.      { if(ccd_flag==1)//采完一副图像
  194.        {
  195.          ccd_flag=0;
  196.          tuxiang();  //红树伟业上位机
  197.          discnt++;
  198.          if(discnt>=100)
  199.          { discnt=0;
  200.            disdata2();  //显示图像
  201.            disccdval();
  202.            delayms(1);
  203.          }
  204.        }
  205.        if(gpio_get(PORTE,3)==0)  //状态切换
  206.        { delayms(4);
  207.          if(gpio_get(PORTE,3)==0)
  208.          {
  209.            workflag=2;
  210.            LCD_CLS(); //清屏
  211.            dispage3();
  212.            while(gpio_get(PORTE,3)==0); //等待按键松开
  213.          }
  214.        }
  215.        if(gpio_get(PORTE,2)==0)  //回正常状态
  216.        { delayms(4);
  217.          if(gpio_get(PORTE,2)==0)
  218.          {
  219.            workflag=0;
  220.            LCD_CLS(); //清屏
  221.            dispage1();
  222.            while(gpio_get(PORTE,5)==0); //等待按键松开
  223.          }
  224.        }
  225.      }
  226.      else if(workflag==2)  //调节参数
  227.      { if(gpio_get(PORTE,3)==0)  //修改参数变换
  228.        { delayms(4);
  229.          if(gpio_get(PORTE,3)==0)
  230.          {
  231.            paraadjflag++;
  232.            if(paraadjflag==5) paraadjflag=0;  // 切换。如果有更多的参数组，自行修改
  233.            if(paraadjflag==0)
  234.            { LCD_P6x8Str(0,1,"Speed Para:  <--");
  235.              LCD_P6x8Str(0,3,"Turn Para:      ");
  236.              LCD_P6x8Str(0,5,"Speed Set:      ");
  237.            }
  238.            else if(paraadjflag==1)
  239.            { LCD_P6x8Str(0,1,"Speed Para:  -->");
  240.              LCD_P6x8Str(0,3,"Turn Para:      ");
  241.              LCD_P6x8Str(0,5,"Speed Set:      ");
  242.            }  
  243.            else if(paraadjflag==2)
  244.            { LCD_P6x8Str(0,1,"Speed Para:     ");
  245.              LCD_P6x8Str(0,3,"Turn Para:   <--");
  246.              LCD_P6x8Str(0,5,"Speed Set:      ");
  247.            }
  248.            else if(paraadjflag==3)
  249.            { LCD_P6x8Str(0,1,"Speed Para:     ");
  250.              LCD_P6x8Str(0,3,"Turn Para:   -->");
  251.              LCD_P6x8Str(0,5,"Speed Set:      ");
  252.            }
  253.            else if(paraadjflag==4)
  254.            { LCD_P6x8Str(0,1,"Speed Para:     ");
  255.              LCD_P6x8Str(0,3,"Turn Para:      ");
  256.              LCD_P6x8Str(0,5,"Speed Set:   <--");
  257.            }
  258.            while(gpio_get(PORTE,3)==0); //等待按键松开
  259.          }
  260.        }
  261.        if(gpio_get(PORTE,2)==0)  //回正常状态
  262.        { delayms(4);
  263.          if(gpio_get(PORTE,2)==0)
  264.          {
  265.            workflag=0;
  266.            LCD_CLS(); //清屏
  267.            dispage1();
  268.            while(gpio_get(PORTE,2)==0); //等待按键松开
  269.          }
  270.        }
  271.        if(gpio_get(PORTE,0)==0)  //参数调节
  272.        { delayms(4);
  273.          if(gpio_get(PORTE,0)==0)
  274.          { if(paraadjflag==0)   //速度KP 加减
  275.            { SpeedKP++;
  276.              Dis_num(24,2,SpeedKP);
  277.            }
  278.            else if(paraadjflag==1)
  279.            { SpeedKI++;
  280.              Dis_num(90,2,SpeedKI);         
  281.            }
  282.            else if(paraadjflag==2)
  283.            { turnsetmax=turnsetmax+5;
  284.              Dis_num(24,4,turnsetmax);         
  285.            }
  286.            else if(paraadjflag==3)
  287.            { TurnD++;
  288.              Dis_num(90,4,TurnD);         
  289.            }
  290.            else if(paraadjflag==4)  //速度
  291.            { SetSpeed = SetSpeed - 5;
  292.              AmSpeed = SetSpeed;
  293.              Dis_num(24,6,SetSpeed);
  294.            }
  295.            while(gpio_get(PORTE,0)==0); //等待按键松开
  296.          }
  297.        }
  298.        if(gpio_get(PORTE,1)==0)  //参数调节
  299.        { delayms(4);
  300.          if(gpio_get(PORTE,1)==0)
  301.          {
  302.            if(paraadjflag==0)   //速度KP 加减
  303.            { SpeedKP--;
  304.              Dis_num(24,2,SpeedKP);
  305.            }
  306.            else if(paraadjflag==1)
  307.            { SpeedKI--;
  308.              Dis_num(90,2,SpeedKI);         
  309.            }
  310.            else if(paraadjflag==2)
  311.            { turnsetmax=turnsetmax-5;
  312.              Dis_num(24,4,turnsetmax);         
  313.            }
  314.            else if(paraadjflag==3)
  315.            { TurnD--;
  316.              Dis_num(90,4,TurnD);         
  317.            }
  318.            else if(paraadjflag==4)  //速度
  319.            { SetSpeed = SetSpeed + 5;
  320.              Dis_num(24,6,SetSpeed);
  321.              AmSpeed = SetSpeed;
  322.            }
  323.            while(gpio_get(PORTE,1)==0); //等待按键松开
  324.          }
  325.        }
  326.        delayms(5);
  327.      }
  328.    }
  329. }

  331. void GPIO_Init()
  332. {
  333.    gpio_init (PORTA, 17, GPO, 1u); //初始化PTE0为高电平输出---LED0
  334.    gpio_set  (PORTA, 19, 0);       //设置PTE0为高电平输出，LED0灭
  335.    gpio_init (PORTE,0,GPI,0);      //按键输入
  336.    gpio_init (PORTE,1,GPI,0);     //按键输入
  337.    gpio_init (PORTE,2,GPI,0);     //按键输入
  338.    gpio_init (PORTE,3,GPI,0);     //按键输入   
  339.    
  340. }

  342. void run()//直立函数 在isr.c中的1ms中断中调用  PIT0_IRQHandler
  343. {   
  344.   TimeCount++ ;
  345.   SpeedPeriodCount++;
  346.   TurnPeriodCount ++ ;
  347.   AAngPeriodCount ++ ;
  348.   if(AAngPeriodCount>=4) AAngPeriodCount=4;
  349.   
  350.   MotorTurnPWM  = TurnPWMOut(TurnPWMOUT,LastTurnPWMOUT,TurnPeriodCount) ;
  351.   MotorSpeedPWM = SpeedPWMOut(g_SpeedControlOutNew ,g_SpeedControlOutOld,SpeedPeriodCount);  
  352.   MotorAAngPWM  = AAangPWMOut(AAngPWM ,LastAAngPWM ,AAngPeriodCount);  
  353.   Checkcarstate();//开启停止判断
  354.   if(TimeCount>=5)//读速度  5ms
  355.   {
  356.     TimeCount=0;
  357.     GetMotorPulse();//读速度脉冲
  358.   }  
  359.   else if(TimeCount == 1)//读取MPU6050
  360.   {      
  361.      xaout=MPU6050_GetDoubleData(MPU6050_ACCEL_XOUT);//读X轴加速度
  362.      yaout=MPU6050_GetDoubleData(MPU6050_ACCEL_YOUT);//读X轴加速度
  363.      zaout=MPU6050_GetDoubleData(MPU6050_ACCEL_ZOUT);//读X轴加速度
  364.      xgout=MPU6050_GetDoubleData(MPU6050_GYRO_XOUT);//读X轴角速度,直立角速度
  365.      ygout=MPU6050_GetDoubleData(MPU6050_GYRO_YOUT);//读Y轴角速度，转向角速度
  366.      angle6550=MPU6050_Get_Angle(xaout,yaout,zaout,0);
  367.   }
  368.   else if(TimeCount == 2)//5ms 直立滤波，控制
  369.   {
  370.     AAngPeriodCount  = 0;
  371.     AngleCalculate();//计算加速值和角度值
  372.     LastAAngPWM = AAngPWM ;      
  373.     AAngPWM = AngleControl() ;     //计算平衡电机速度
  374.     PWMout=MotorSpeedOut(AAngPWM,MotorSpeedPWM,MotorTurnPWM);//电机输出
  375.     //PWMout=MotorSpeedOut(0,0,MotorTurnPWM);
  376.     //PWMout=MotorSpeedOut(AAngPWM,MotorSpeedPWM,0);//电机输出  刚开始用的时候可以把MotorSpeedPWM和MotorTurnPWM都设为0
  377.      //PWMout=MotorSpeedOut(AAngPWM,0,0);//电机输出  刚开始用的时候可以把MotorSpeedPWM和MotorTurnPWM都设为0
  378.     //先调直立，再加速度MotorSpeedPWM，最后加方向MotorTurnPWM
  379.    
  380.   }
  381.   else if(TimeCount == 3)//5ms  速度PI调节
  382.   {
  383.      ATimeCount ++ ;
  384.      if(ATimeCount >= 20)//20*5=100ms进行一次速度PID调节
  385.      {
  386.        ATimeCount=0;
  387.        SpeedPID() ;
  388.        SpeedPeriodCount = 0 ;
  389.        qibu();//起步  不可以一下就把速度加上，要缓慢加
  390.      }
  391.    }
  392.   else if(TimeCount == 4)// CCD图像采集与处理
  393.   {
  394.     ccd_count++;
  395.     if(ccd_count>=4)//20ms进一次
  396.     {
  397.         ccd_count=0;
  398.         ccd_flag=1;//发送图像到上位机标志
  399.         ImageCapture(Pixel) ;//将捕捉到的图像放在Pixel数组中
  400.         //process();//图像处理函数
  401.         FindLine();
  402.         dline = rightline - leftline;
  403.         if((dline>64)||(dline<12)) //十字路或桥上
  404.         { //TurnP = TurnP1;
  405.           //TurnD = TurnD1;
  406.           turnpwmmax=turnsetmax;
  407.         }
  408.         else if((dline>(whitelength+16))||(dline<(whitelength-16))) //丢线
  409.         { turnpwmmax=turnsetmax+50;
  410.           //TurnD = TurnD4;
  411.         }
  412.         else if((dline>(whitelength+9))||(dline<(whitelength-9))) //60度弯道
  413.         { //TurnP = TurnP4;
  414.           //TurnD = TurnD4;
  415.           turnpwmmax=turnsetmax+30;
  416.         }
  417.         else if((dline>(whitelength+4))||(dline<(whitelength-4))) //50度弯道
  418.         { //TurnP = TurnP3;
  419.           //TurnD = TurnD3;
  420.           turnpwmmax=turnsetmax+20;
  421.         }
  422.          else if((dline>(whitelength+2))||(dline<(whitelength-2))) //亚直道
  423.         { //TurnP = TurnP2;
  424.           //TurnD = TurnD2;
  425.           turnpwmmax=turnsetmax;
  426.         }
  427.         else //直道
  428.         { //TurnP = TurnP1;
  429.           //TurnD = TurnD1;
  430.           turnpwmmax=turnsetmax;
  431.         }
  432.         LastTurnPWMOUT = TurnPWMOUT ;
  433.         TurnPWM() ;
  434.         TurnPeriodCount = 0 ;
  435.     }
  436.   }  
  437.   if(TIME1flag_1s == 1)//程序运行灯
  438.   {
  439.     TIME1flag_1s = 0 ;
  440.     PTA17_OUT = ~PTA17_OUT ;
  441.   }
  442. }
  443. void qibu()//这一部分认真看一下，也可以根据自己的想法改一下
  444. {
  445.   if(AmSpeed!=0)//在目标速度不为0时才运行
  446.   {
  447.    start_flag++;
  448.    if(start_flag<30)//静止2S才出发  可根据规则修改
  449.    {  g_Speedgoal=0;
  450.       //SpeedKI=0;
  451.    }
  452.    else if(start_flag>=30)
  453.    {
  454.      if(g_Speedgoal>AmSpeed&&stop_jiasu==0)//然后逐渐加速
  455.      {
  456.          g_Speedgoal-=15; //  30  //单次加速值，值大加速能力强，比赛前可设两组值，一组快速加速，用于应对起步时是长
  457.                          //直道的情况，另一组为慢加速，用于应对起步时就是弯道的情况
  458.      }
  459.      else if(g_Speedgoal<=AmSpeed&&stop_jiasu==0) //内部速度高于设置速度，停止大幅度加速，进行小幅度调整加速
  460.      {
  461.          g_Speedgoal=AmSpeed;
  462.          stop_jiasu=1;
  463.      }                                
  464.      start_flag=3000;  //终止静止2s计数
  465.      //SpeedKI=0;
  466.      if(stop_jiasu==1)        //车模之后的目标速度调整主要在此函数中
  467.      {
  468.        if(g_Speedgoal>AmSpeed)//然后逐渐加速
  469.        {
  470.          g_Speedgoal-=15;//
  471.        }
  472.         else if(g_Speedgoal<=AmSpeed)
  473.        {
  474.          g_Speedgoal=AmSpeed;
  475.        }   
  476.      }
  477.    }
  478.   }
  479. }


  482. void dispage1()
  483. {  Dis_num(0,0,GRAVITY_OFFSET);  //设定值，垂直时加速度AD
  484.    Dis_num(42,0,GYROSCOPE_OFFSET);//陀螺仪零偏  自检平均值
  485.    LCD_P6x8Str(0,1,"Ap:");
  486.    Dis_num(20,1,A_P);//加计零偏 定值
  487.    LCD_P6x8Str(60,1,"Ad:");
  488.    Dis_num(80,1,A_D);
  489.    
  490.    LCD_P6x8Str(0,2,"Vp:");
  491.    Dis_num(20,2,V_P);
  492.    LCD_P6x8Str(60,2,"Vd:");
  493.    Dis_num(80,2,V_I);
  494.    
  495.    LCD_P6x8Str(0,3,"Tp:");
  496.    Dis_num(20,3,T_P);
  497.    LCD_P6x8Str(60,3,"Td:");
  498.    Dis_num(80,3,T_D);
  499.    
  500.    LCD_P6x8Str(0,4,"A      S      C");
  501.    
  502.    LCD_P6x8Str(0,5,"Spd");
  503.    LCD_P6x8Str(1,7,"LMB:");
  504.    LCD_P6x8Str(64,7,"RMB:");
  505. }

  507. void disdata1()
  508. {    Dis_num(6,4,angletmp);
  509.      Dis_num(48,4,-AmSpeed);
  510.      Dis_num(90,4,TurnPosition);
  511.      A_P = ANGLE_CONTROL_P;
  512.      A_D = ANGLE_CONTROL_D*10;
  513.      V_P = SpeedKP;
  514.      V_I = SpeedKI;  
  515.      T_P = TurnP;
  516.      T_D = TurnD;  
  517.      Dis_num(20,1,A_P);//加计零偏 定值
  518.      //Dis_num(80,1,A_D);
  519.      Dis_num(80,1,TurnPWMOUT);
  520.      Dis_num(20,2,V_P);
  521.      Dis_num(80,2,V_I);
  522.      Dis_num(20,3,T_P);
  523.      Dis_num(80,3,T_D);
  524.      delayms(50);
  525.      Dis_num(10,6,leftlineold);  //赛道左边界
  526.      Dis_num(94,6,rightlineold); //赛道右边界
  527.      Dis_num(52,6,dline); //赛道右-左
  528.      Dis_num(28,7,sumlspeed);
  529.      Dis_num(96,7,sumrspeed);
  530.      Dis_num(20,5,AmSpeed);
  531.      Dis_num(80,5,turnsetmax);
  532. }

  534. void dispage2()
  535. {  LCD_P6x8Str(0,0,"CCD DATA UP&DIS");
  536.    LCD_P6x8Str(0,1,"L:        R:");
  537.    Dis_num(20,1,leftlineold);  //赛道左边界
  538.    Dis_num(94,1,rightlineold); //赛道右边界
  539. }

  541. void disdata2()
  542. {  Dis_num(20,1,leftlineold);  //赛道左边界
  543.    Dis_num(94,1,rightlineold); //赛道右边界
  544. }
  545. ……………………

  547. …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码

所有资料51hei提供下载:
K60_CCD直立.rar (13.35 MB, 下载次数: 55)



分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏1 分享淘帖 顶顶 踩踩
回复

使用道具 举报

沙发
ID:1 发表于 2018-5-12 06:08 | 只看该作者
好资料，51黑有你更精彩!!!
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表