標題: K60_CCD直立小車源碼(二等獎程序) [打印本頁]
作者: zoulichuan    時間: 2018-5-10 21:07
標題: K60_CCD直立小車源碼(二等獎程序)
k60 ccd二等獎程序

本程序配套4按鍵直立一體板

按鍵功能：
E0： 參數(shù)加
E1： 參數(shù)減
E2： 切換顯示及功能
E3： 回到最初顯示及功能


調(diào)試直立

A、修改直立靜態(tài)偏移量： calcultion.c   s32 GRAVITY_OFFSET=485;//230  MPU6550為平衡時Z軸角度，放大10倍
B、main.c里 run()函數(shù)中：  修改  PWMout=MotorSpeedOut(AAngPWM,MotorSpeedPWM,MotorTurnPWM);//電機輸出
                            為： PWMout=MotorSpeedOut(AAngPWM,0,0);//關(guān)閉速度環(huán)和轉(zhuǎn)向環(huán)分量，直立后再加上兩個分量



系統(tǒng)連線說明 ：


單片機源程序如下:
  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六軸參數(shù)  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;  //死區(qū)電壓左路電機
  29. uint8 PWMDEADR = 0;  //死區(qū)電壓右路電機

  31. uint16 MAXTURNPWM= 200;  //電機轉(zhuǎn)向最大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中讀出的參數(shù)
  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;  //彎道時舵機轉(zhuǎn)角比例系數(shù) 放大10倍
  49. //u16 TurnD2 = 140;//30;  //彎道時舵機轉(zhuǎn)角積分系數(shù)  放大100倍
  50. //
  51. //u16 TurnP3 = 40;  //大彎時轉(zhuǎn)角比例
  52. //u16 TurnD3 = 110;   //大彎時轉(zhuǎn)角積分
  53. //
  54. //u16 TurnP4 = 48;  //大彎時轉(zhuǎn)角比例
  55. //u16 TurnD4 = 90;   //大彎時轉(zhuǎn)角積分

  57. u16 TurnP;     //* 94                 //方向P
  58. u16 TurnD;   //* 8.4              //方向D
  59. u16 turnsetmax;  //轉(zhuǎn)向最大PWM值
  60. u16 turnpwmmax;
  61. u8  disccd[128][4];
  62. u8  paraadjflag;   //參數(shù)修改組切換


  65. extern int leftline, rightline; //左右邊界值
  66. extern int leftlineold,rightlineold; //左右邊界上一次值
  67. extern int yuzhizhi;//動態(tài)閾值
  68. extern signed int sumlspeed,sumrspeed;

  70. s16 SetSpeed= -250;
  71. s16 AmSpeed;//*   //目標速度 用于外部       設(shè)置速度時在這里設(shè)置
  72. s16 g_Speedgoal=0;//用于內(nèi)部 自加速使用
  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構(gòu)成，速度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;//轉(zhuǎn)向陀螺儀
  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();//直立主函數(shù)
  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軸角速度，轉(zhuǎn)向
  151.      Pause();
  152.    }
  153.    GYROSCOPE_OFFSET = xgtemp/100;        //靜態(tài)X軸角速度值，直立，理論上為0，但實際可能不為0
  154.    GYROSCOPE_turn_OFFSET = ygtemp/100;   //靜態(tài)Y軸角速度值，轉(zhuǎn)向，理論上為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)  //正常狀態(tài)
  174.      { if(gpio_get(PORTE,3)==0)  //狀態(tài)切換
  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();  //紅樹偉業(yè)上位機
  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)  //狀態(tài)切換
  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)  //回正常狀態(tài)
  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)  //調(diào)節(jié)參數(shù)
  227.      { if(gpio_get(PORTE,3)==0)  //修改參數(shù)變換
  228.        { delayms(4);
  229.          if(gpio_get(PORTE,3)==0)
  230.          {
  231.            paraadjflag++;
  232.            if(paraadjflag==5) paraadjflag=0;  // 切換。如果有更多的參數(shù)組，自行修改
  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)  //回正常狀態(tài)
  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)  //參數(shù)調(diào)節(jié)
  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)  //參數(shù)調(diào)節(jié)
  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);       //設(shè)置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()//直立函數(shù) 在isr.c中的1ms中斷中調(diào)用  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軸角速度，轉(zhuǎn)向角速度
  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都設(shè)為0
  377.      //PWMout=MotorSpeedOut(AAngPWM,0,0);//電機輸出  剛開始用的時候可以把MotorSpeedPWM和MotorTurnPWM都設(shè)為0
  378.     //先調(diào)直立，再加速度MotorSpeedPWM，最后加方向MotorTurnPWM
  379.    
  380.   }
  381.   else if(TimeCount == 3)//5ms  速度PI調(diào)節(jié)
  382.   {
  383.      ATimeCount ++ ;
  384.      if(ATimeCount >= 20)//20*5=100ms進行一次速度PID調(diào)節(jié)
  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;//發(fā)送圖像到上位機標志
  399.         ImageCapture(Pixel) ;//將捕捉到的圖像放在Pixel數(shù)組中
  400.         //process();//圖像處理函數(shù)
  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()//這一部分認真看一下，也可以根據(jù)自己的想法改一下
  444. {
  445.   if(AmSpeed!=0)//在目標速度不為0時才運行
  446.   {
  447.    start_flag++;
  448.    if(start_flag<30)//靜止2S才出發(fā)  可根據(jù)規(guī)則修改
  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  //單次加速值，值大加速能力強，比賽前可設(shè)兩組值，一組快速加速，用于應(yīng)對起步時是長
  457.                          //直道的情況，另一組為慢加速，用于應(yīng)對起步時就是彎道的情況
  458.      }
  459.      else if(g_Speedgoal<=AmSpeed&&stop_jiasu==0) //內(nèi)部速度高于設(shè)置速度，停止大幅度加速，進行小幅度調(diào)整加速
  460.      {
  461.          g_Speedgoal=AmSpeed;
  462.          stop_jiasu=1;
  463.      }                                
  464.      start_flag=3000;  //終止靜止2s計數(shù)
  465.      //SpeedKI=0;
  466.      if(stop_jiasu==1)        //車模之后的目標速度調(diào)整主要在此函數(shù)中
  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);  //設(shè)定值，垂直時加速度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黑下載附件…………
復(fù)制代碼

所有資料51hei提供下載:
K60_CCD直立.rar (13.35 MB, 下載次數(shù): 55)



作者: admin    時間: 2018-5-12 06:08
好資料，51黑有你更精彩!!!



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1