標題: 第七屆工訓賽物流組小車電控程序 [打印本頁]
作者: 愛慕春妮    時間: 2021-8-6 12:50
標題: 第七屆工訓賽物流組小車電控程序
第七屆工訓賽物流組資料（小車電控程序）
  1. #include "stm32f10x.h"
  2. #include "delay.h"
  3. #include "usart.h"
  4. #include "led.h"
  5. #include "timer.h"
  6. #include "encoder.h"
  7. #include "motor.h"
  8. #include "dataprocess.h"
  9. #include "steer.h"
  10. #include "show.h"
  11. #include "oled.h"
  12. #include "exti.h"
  13. #include "DataScope_DP.h"
  14. #include "adc.h"
  15. #include "dma.h"
  16. #include "steercontrol.h"


  19. u8 Mode_All=1;                                                                                                        //總模式標志，0，控制電機，1，控制舵機
  20. u16 Time_Count=0;                                                                                        //時間計數變量
  21. u16 Distance_F,Distance_L;                                                //前方距離和左邊距離

  23. u16 LocationInit_X,LocationInit_Y;                //初始坐標
  24. int Location_X,Location_Y;                                                //當前坐標
  25. float Yaw ;                                                                                                                //航向角

  27. int Encoder_Left,Encoder_Right;
  28. int Distance_Left=0,Distance_Right=0;        //用于更新坐標（單位時間內【50ms】左輪前進距離和右輪前進距離）
  29. u8 state=1;                                                                                                                //mode=0,停止；mode=1,直行；mode=2,右轉90度；mode =3,左轉90度；mode=4，左轉180度
  30. u16 Voltage;                                                                                                        //電池電壓
  31. u16 AD_Value[2];                                                                                        //電池電壓ADC轉換的結果

  33. u8 Turn_Flag;                                                                                                        //轉向標志，如果某一次轉向完成，則置1，反之，置0

  35. u8 Task=2;                                                                                                                //通過掃描二維碼得到的任務

  37. unsigned char FSM_Status = 0;                                        //狀態(tài)機的狀態(tài)變量

  39. float Target1=750,Target2=750,Target3=750,Target4=750,Target5=835,Target6=750;     //電機目標值
  40. float TargetX=0.2,TargetY=0,Target_Beta=-10,Target_Alpha=0,Target_Gamma=-5;   //姿態(tài)目標值，Target_Beta 1號舵機旋轉角度，Target_Alpha，云臺角度，Target_Gamma，6號舵機角度
  41. u16 Angle1=90,Angle2=90,Angle3=90,Angle4=90,Angle5=90,Angle6=105;

  43. int main(void)
  44. {
  45.         u16 i;
  46.         delay_init();                                                                //延時函數初始化
  47.         NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
  48.         LED_Init();                                                                        //LED初始化
  49.         OLED_Init();                                                                //OLED初始化
  50.         uart_init(128000);                                        //串口一初始化
  51. //        uart4_init(9600);                                                //測距串口初始化
  52. //        uart5_init(115200);                                        //openmv串口初始化
  53. //        AdcInit();                                                                        //電池電壓采集
  54. //        MYDMA_Config();                                                        //DMA1采集ADC
  55. //        IIC_Init();                 //=====模擬IIC初始化
  56. //  MPU6050_initialize();       //=====MPU6050初始化
  57. //  DMP_Init();                 //=====初始化DMP
  58.         
  59.         Encoder_Init_TIM2();                                //編碼器初始化        ，右輪，為正值
  60.         Encoder_Init_TIM3();                                //編碼器初始化        ，左輪，為負值
  61.         Motor_Init();                                                                //電機控制IO口初始化
  62.         Motor_PWM_Init(7199,0);                        //電機pwm初始化，10KHz
  63.         TIM1_PWM_Init(9999,144);    //舵機PWM初始化，50Hz,T=20ms
  64.         TIM8_PWM_Init(9999,144);    //舵機PWM初始化，50Hz,T=20ms
  65. //        TIM6_Int_Init(499,7200);                //50ms定時中斷
  66.         TIM7_Int_Init(99,7200);                //10ms定時中斷
  67.         EXTIX_Init();                                                                //按鍵外部中斷初始化
  68. //        EXTI_PC5_Init();                                                //mpu6050INT引腳中斷初始化

  70. //        SetSteer(95,90,90,90,90,105);
  71. //        delay_ms(1000);
  72. //        delay_ms(1000);

  74. //        SetSteer_PID(95,155,90,180,90,130);

  76.         while(1)
  77.         {
  78.                 oled_show();               
  79.                 delay_ms(50);


  82.         }
  83.         while(1)
  84.         {
  85.                 state =1;//直行
  86.                 oled_show();
  87.                 if(Location_X == 0 && Location_Y >= 200)
  88.                 {
  89.                         state=0;//停
  90.                         break;
  91.                 }
  92.         }
  93.         for(i=0;i<20;i++)
  94.         {
  95.                 oled_show();
  96.                 delay_ms(50);
  97.         }
  98.         while(1)
  99.         {
  100.                 state = 2;        //右轉90                90
  101.                 oled_show();
  102.                 if(Turn_Flag == 1)//轉向完成
  103.                 {
  104.                         state = 0;        //停
  105.                         Turn_Flag =0;
  106.                         break;
  107.                 }
  108.         }
  109.         delay_ms(300);
  110.         while(1)
  111.         {
  112.                 state = 1;//直行
  113.                 oled_show();
  114.                 if(Location_Y >= 200 && Location_X >= 1800)
  115.                 {
  116.                         state =0;
  117.                         break;
  118.                 }
  119.         }
  120.         delay_ms(300);
  121.         while(1)
  122.         {
  123.                 state = 2;        //右轉90                180
  124.                 oled_show();
  125.                 if(Turn_Flag == 1)//轉向完成
  126.                 {
  127.                         state = 0;        //停
  128.                         Turn_Flag =0;
  129.                         break;
  130.                 }
  131.         }
  132.         delay_ms(300);
  133.         while(1)
  134.         {
  135.                 state = 2;        //右轉90                        -90
  136.                 oled_show();
  137.                 if(Turn_Flag == 1)//轉向完成
  138.                 {
  139.                         state = 0;        //停
  140.                         Turn_Flag =0;
  141.                         break;
  142.                 }
  143.         }
  144.         delay_ms(300);
  145.         while(1)
  146.         {
  147.                 state = 1;//直行
  148.                 oled_show();
  149.                 if(Location_Y >= 200 && Location_X <= 900)
  150.                 {
  151.                         state =0;
  152.                         break;
  153.                 }
  154.         }
  155.         while(1)
  156.         {
  157.                 state = 2;        //右轉90                        0
  158.                 oled_show();
  159.                 if(Turn_Flag == 1)//轉向完成
  160.                 {
  161.                         state = 0;        //停
  162.                         Turn_Flag =0;
  163.                         break;
  164.                 }
  165.         }
  166.         delay_ms(300);
  167.         while(1)
  168.         {
  169.                 state = 4;//左轉180                                                -180
  170.                 oled_show();
  171.                 if(Turn_Flag == 1)//轉向完成
  172.                 {
  173.                         state = 0;        //停
  174.                         Turn_Flag =0;
  175.                         break;
  176.                 }
  177.         }
  178.         delay_ms(300);
  179.         while(1)
  180.         {
  181.                 state = 2;        //右轉90                                                -90
  182.                 oled_show();
  183.                 if(Turn_Flag == 1)//轉向完成
  184.                 {
  185.                         state = 0;        //停
  186.                         Turn_Flag =0;
  187.                         break;
  188.                 }
  189.         }
  190.         delay_ms(300);
  191.         while(1)
  192.         {
  193.                 state = 1;//直行
  194.                 oled_show();
  195.                 if(Location_Y >= 200 && Location_X <= 0)
  196.                 {
  197.                         state =0;
  198.                         break;
  199.                 }
  200.         }
  201.         while(1)
  202.         {
  203.                 state = 3;        //左轉90                                        -180
  204.                 oled_show();
  205.                 if(Turn_Flag == 1)//轉向完成
  206.                 {
  207.                         state = 0;        //停
  208.                         Turn_Flag =0;
  209.                         break;
  210.                 }
  211.         }
  212.         delay_ms(300);
  213.         while(1)
  214.         {
  215.                 state = 1;//直行
  216.                 oled_show();
  217.                 if(Location_Y <= 0 && Location_X <= 0)
  218.                 {
  219.                         state =0;
  220.                         break;
  221.                 }
  222.         }
  223.         while(1)
  224.         {
  225.                 Led_Flash(50000);
  226.                 oled_show();
  227.                 delay_ms(50);
  228.         }
  229. }

復制代碼

代碼下載,如有錯誤,請指正: stm32程序.7z (313.44 KB, 下載次數: 25)




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