API tools faq
paste
Advertisement
safwan092

wifi_robot

safwan092
May 6th, 2017
343
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 16.52 KB | None | 0 0
raw download clone embed print report
  1. /*
  2. * Copyright (c) 2015,小二极客科技有限公司
  3. * All rights reserved.
  4. *
  5. * 文件名称：wifi-robots
  6. * 文件标识：
  7. * 摘    要：wifi机器人智能小车控制
  8. *
  9. * 当前版本：1.0
  10. * 作    者：小R团队
  11. * 完成日期：2015年4月1日
  12. */
  13. #include <Servo.h>
  14. //#include <MsTimer2.h>
  15. #include <EEPROM.h>
  16.  
  17. int ledpin = 13;//设置系统启动指示灯
  18. int ENA = 5;//L298使能A
  19. int ENB = 6;//L298使能B
  20. int INPUT2 = 7;//电机接口1
  21. int INPUT1 = 8;//电机接口2
  22. int INPUT3 = 12;//电机接口3
  23. int INPUT4 = 13;//电机接口4
  24. int num;//定义电机标志位
  25.  
  26. int Echo = A5;  // 定义超音波信号接收脚位  
  27. int Trig = A4;  // 定义超音波信号发射脚位
  28. int Input_Detect_LEFT = A3;    //定义小车左侧红外
  29. int Input_Detect_RIGHT = A2;  //定义小车右侧红外
  30. int Input_Detect = A1;//定义小车前方红外
  31. int Carled = A0;//定义小车车灯接口
  32. int Cruising_Flag = 0;
  33. int Pre_Cruising_Flag = 0 ;
  34. int Left_Speed_Hold = 200;//定义左侧速度变量
  35. int Right_Speed_Hold = 200;//定义右侧速度变量
  36.  
  37.  
  38. #define MOTOR_GO_FORWARD  {digitalWrite(INPUT1,LOW);digitalWrite(INPUT2,HIGH);digitalWrite(INPUT3,LOW);digitalWrite(INPUT4,HIGH);}    //车体前进                               
  39. #define MOTOR_GO_BACK     {digitalWrite(INPUT1,HIGH);digitalWrite(INPUT2,LOW);digitalWrite(INPUT3,HIGH);digitalWrite(INPUT4,LOW);}    //车体前进
  40. #define MOTOR_GO_RIGHT    {digitalWrite(INPUT1,HIGH);digitalWrite(INPUT2,LOW);digitalWrite(INPUT3,LOW);digitalWrite(INPUT4,HIGH);}    //车体前进
  41. #define MOTOR_GO_LEFT     {digitalWrite(INPUT1,LOW);digitalWrite(INPUT2,HIGH);digitalWrite(INPUT3,HIGH);digitalWrite(INPUT4,LOW);}    //车体前进
  42. #define MOTOR_GO_STOP     {digitalWrite(INPUT1,LOW);digitalWrite(INPUT2,LOW);digitalWrite(INPUT3,LOW);digitalWrite(INPUT4,LOW);}    //车体前进
  43.  
  44.  
  45. void forward(int num)
  46. {
  47.     switch(num)
  48.     {
  49.         case 1:MOTOR_GO_FORWARD;return;
  50.         case 2:MOTOR_GO_FORWARD;return;
  51.         case 3:MOTOR_GO_BACK;return;
  52.         case 4:MOTOR_GO_BACK;return;
  53.         case 5:MOTOR_GO_LEFT;return;
  54.         case 6:MOTOR_GO_LEFT;return;
  55.         case 7:MOTOR_GO_RIGHT;return;
  56.         case 8:MOTOR_GO_RIGHT;return;
  57.         default:return;    
  58.     }
  59. }
  60.  
  61. void back(int num)
  62. {
  63.         switch(num)
  64.     {
  65.         case 1:MOTOR_GO_BACK;return;
  66.         case 2:MOTOR_GO_BACK;return;
  67.         case 3:MOTOR_GO_FORWARD;return;
  68.         case 4:MOTOR_GO_FORWARD;return;
  69.         case 5:MOTOR_GO_RIGHT;return;
  70.         case 6:MOTOR_GO_RIGHT;return;
  71.         case 7:MOTOR_GO_LEFT;return;
  72.         case 8:MOTOR_GO_LEFT;return;
  73.         default:return;    
  74.     }
  75. }
  76. void left(int num)
  77. {
  78.         switch(num)
  79.     {
  80.         case 1:MOTOR_GO_LEFT;return;
  81.         case 2:MOTOR_GO_RIGHT;return;
  82.         case 3:MOTOR_GO_LEFT;return;
  83.         case 4:MOTOR_GO_RIGHT;return;
  84.         case 5:MOTOR_GO_FORWARD;return;
  85.         case 6:MOTOR_GO_BACK;return;
  86.         case 7:MOTOR_GO_FORWARD;return;
  87.         case 8:MOTOR_GO_BACK;return;
  88.         default:return;
  89.     }
  90. }
  91. void right(int num)
  92. {
  93.         switch(num)
  94.     {
  95.         case 1:MOTOR_GO_RIGHT;return;
  96.         case 2:MOTOR_GO_LEFT;return;
  97.         case 3:MOTOR_GO_RIGHT;return;
  98.         case 4:MOTOR_GO_LEFT;return;
  99.         case 5:MOTOR_GO_BACK;return;
  100.         case 6:MOTOR_GO_FORWARD;return;
  101.         case 7:MOTOR_GO_BACK;return;
  102.         case 8:MOTOR_GO_FORWARD;return;
  103.         default:return;
  104.     }
  105. }
  106.  
  107.  
  108. int Left_Speed[11]={90,106,122,138,154,170,186,203,218,234,255};//左侧速度档位值
  109. int Right_Speed[11]={90,106,122,138,154,170,186,203,218,234,255};//右侧速度档位值
  110.  
  111. //Servo servo1;// 创建舵机#1号
  112. //Servo servo2;// 创建舵机#2号
  113. Servo servo3;// 创建舵机#3号
  114. Servo servo4;// 创建舵机#4号
  115. Servo servo5;// 创建舵机#5号
  116. Servo servo6;// 创建舵机#6号
  117. Servo servo7;// 创建舵机#7号
  118. Servo servo8;// 创建舵机#8号
  119.  
  120.  
  121.  
  122. //byte angle1=60;//舵机#1初始值
  123. //byte angle2=60;//舵机#2初始值
  124. byte angle3=60;//舵机#3初始值
  125. byte angle4=60;//舵机#4初始值
  126. byte angle5=60;//舵机#5初始值
  127. byte angle6=120;//舵机#6初始值
  128. byte angle7=60;//舵机#7初始值
  129. byte angle8=60;//舵机#8初始值
  130.  
  131.  
  132. int buffer[3];  //串口接收数据缓存
  133. int rec_flag;   //串口接收标志位
  134. int serial_data;
  135. int Uartcount;
  136. int IR_R;
  137. int IR_L;
  138. int IR;
  139. unsigned long Pretime;
  140. unsigned long Nowtime;
  141. unsigned long Costtime;
  142. float Ldistance;
  143.  
  144. void Open_Light()//开大灯
  145.     {      
  146.       digitalWrite(Carled,HIGH);   //拉低电平，正极接电源，负极接Io口
  147.       delay(1000);            
  148.     }
  149. void Close_Light()//关大灯
  150.     {  
  151.       digitalWrite(Carled, LOW);   //拉低电平，正极接电源，负极接Io口
  152.       delay(1000);            
  153.     }
  154.    
  155. void  Avoiding()//红外避障函数
  156.     {  
  157.       IR = digitalRead(Input_Detect);
  158.        if((IR == HIGH))
  159.        {
  160.           forward(num);;//直行
  161.           return;            
  162.        }
  163.        if((IR == LOW))
  164.        {
  165.             MOTOR_GO_STOP;//停止
  166.             return;
  167.        }
  168.     }
  169.            
  170.    
  171.  
  172. void FollowLine()   // 巡线模式
  173.     {  
  174.       IR_L = digitalRead(Input_Detect_LEFT);//读取左边传感器数值
  175.       IR_R = digitalRead(Input_Detect_RIGHT);//读取右边传感器数值
  176.      
  177.       if((IR_L == LOW) && (IR_R == LOW))//两边同时探测到障碍物
  178.       {
  179.         forward(num);//直行
  180.         return;          
  181.        
  182.       }
  183.       if((IR_L == LOW) && (IR_R == HIGH))//右侧遇到障碍  
  184.       {
  185.         left(num);//左转
  186.         return;
  187.        
  188.       }
  189.       if((IR_L == HIGH) &&( IR_R == LOW))//左侧遇到障碍
  190.       {
  191.         right(num);//右转
  192.         return;
  193.        
  194.       }
  195.       if((IR_L == HIGH) && (IR_R == HIGH))//左右都检测到，就如视频中的那样遇到一道横的胶带
  196.       {
  197.         MOTOR_GO_STOP;//停止
  198.         return;
  199.        }
  200.     }    
  201.  
  202. char Get_Distence()//测出距离
  203.  {  
  204.       digitalWrite(Trig, LOW);   // 让超声波发射低电压2μs  
  205.       delayMicroseconds(2);  
  206.       digitalWrite(Trig, HIGH);  // 让超声波发射高电压10μs，这里至少是10μs  
  207.       delayMicroseconds(10);  
  208.       digitalWrite(Trig, LOW);    // 维持超声波发射低电压  
  209.       Ldistance = pulseIn(Echo, HIGH);  // 读差相差时间  
  210.       Ldistance= Ldistance/5.8/10;      // 将时间转为距离距离（单位：公分）    
  211.     //  Serial.println(Ldistance);      //显示距离  
  212.       return Ldistance;
  213.      
  214.   }    
  215.  
  216. void Avoid_wave()//超声波避障函数
  217. {
  218.   Get_Distence();
  219.   if(Ldistance < 15)
  220.       {
  221.           MOTOR_GO_STOP;
  222.       }
  223.       else
  224.       {
  225.            forward(num);
  226.       }
  227. }
  228.  
  229. void Avoid_wave_auto()
  230. {
  231.   Get_Distence();
  232.   if(Ldistance < 15)
  233.   {
  234.     back(num);;
  235.     delay(300);
  236.     MOTOR_GO_STOP;
  237.   }
  238. }
  239.  
  240. void Send_Distance()//超声波距离PC端显示
  241. {
  242.   int dis= Get_Distence();
  243.   Serial.write(0xff);
  244.   Serial.write(0x03);
  245.   Serial.write(0x00);
  246.   Serial.write(dis);
  247.   Serial.write(0xff);
  248.   delay(1000);
  249. }
  250. /*
  251. *********************************************************************************************************
  252. ** 函数名称 ：Delayed()
  253. ** 函数功能 ：延时程序
  254. ** 入口参数 ：无
  255. ** 出口参数 ：无
  256. *********************************************************************************************************
  257. */
  258. void  Delayed()    //延迟40秒等WIFI模块启动完毕
  259. {
  260.     int i;
  261.     for(i=0;i<20;i++)
  262.     {
  263.         digitalWrite(ledpin,LOW);
  264.         delay(1000);
  265.         digitalWrite(ledpin,HIGH);
  266.         delay(1000);
  267.     }
  268. }
  269.  
  270. /*
  271. *********************************************************************************************************
  272. ** 函数名称 ：setup().Init_Steer()
  273. ** 函数功能 ：系统初始化（串口、电机、舵机、指示灯初始化）。
  274. ** 入口参数 ：无
  275. ** 出口参数 ：无
  276. *********************************************************************************************************
  277. */
  278. void Init_Steer()//舵机初始化(角度为上次保存数值)
  279. {
  280.    // angle1 = EEPROM.read(0x01);//读取寄存器0x01里面的值
  281.    // angle2 = EEPROM.read(0x02);//读取寄存器0x02里面的值
  282.     angle3 = EEPROM.read(0x03);//读取寄存器0x03里面的值
  283.     angle4 = EEPROM.read(0x04);//读取寄存器0x04里面的值
  284.     angle5 = EEPROM.read(0x05);//读取寄存器0x05里面的值
  285.     angle6 = EEPROM.read(0x06);//读取寄存器0x06里面的值
  286.     angle7 = EEPROM.read(0x07);//读取寄存器0x07里面的值
  287.     angle8 = EEPROM.read(0x08);//读取寄存器0x08里面的值
  288.    
  289.     if(angle7 == 255 && angle8 == 255)
  290.     {
  291.        // EEPROM.write(0x01,60);//把初始角度存入地址0x01里面
  292.        // EEPROM.write(0x02,60);//把初始角度存入地址0x02里面
  293.         EEPROM.write(0x03,60);//把初始角度存入地址0x03里面
  294.         EEPROM.write(0x04,60);//把初始角度存入地址0x04里面
  295.         EEPROM.write(0x05,60);//把初始角度存入地址0x05里面
  296.         EEPROM.write(0x06,120);//把初始角度存入地址0x06里面
  297.         EEPROM.write(0x07,60);//把初始角度存入地址0x07里面
  298.         EEPROM.write(0x08,60);//把初始角度存入地址0x08里面
  299.         return;
  300.     }
  301.  
  302.    // servo1.write(angle1);//把保存角度赋值给舵机1
  303.    // servo2.write(angle2);//把保存角度赋值给舵机2
  304.     servo3.write(angle3);//把保存角度赋值给舵机3
  305.     servo4.write(angle4);//把保存角度赋值给舵机4
  306.     servo5.write(angle5);//把保存角度赋值给舵机5
  307.     servo6.write(angle6);//把保存角度赋值给舵机6
  308.     servo7.write(angle7);//把保存角度赋值给舵机7
  309.     servo8.write(angle8);//把保存角度赋值给舵机8
  310.     num = EEPROM.read(0x10);//读取寄存器0x10里面的值
  311.     if(num==0xff)EEPROM.write(0x10,1);
  312. }
  313.  
  314. void setup()
  315. {
  316.     pinMode(ledpin,OUTPUT);
  317.     pinMode(ENA,OUTPUT);
  318.     pinMode(ENB,OUTPUT);
  319.     pinMode(INPUT1,OUTPUT);
  320.     pinMode(INPUT2,OUTPUT);
  321.     pinMode(INPUT3,OUTPUT);
  322.     pinMode(INPUT4,OUTPUT);
  323.     pinMode(Input_Detect_LEFT,INPUT);
  324.     pinMode(Input_Detect_RIGHT,INPUT);
  325.     pinMode(Carled, OUTPUT);
  326.     pinMode(Input_Detect,INPUT);
  327.     pinMode(Echo,INPUT);
  328.     pinMode(Trig,OUTPUT);
  329.  
  330.     Delayed();//延迟40秒等WIFI模块启动完毕
  331.     analogWrite(ENB,Left_Speed_Hold);//给L298使能端B赋值
  332.     analogWrite(ENA,Right_Speed_Hold);//给L298使能端A赋值
  333.     digitalWrite(ledpin,LOW);
  334.     //servo1.attach(SDA);//定义舵机1控制口
  335.     //servo2.attach(SCL);//定义舵机2控制口
  336.     servo3.attach(3);//定义舵机3控制口
  337.     servo4.attach(4);//定义舵机4控制口
  338.     servo5.attach(2);//定义舵机5控制口
  339.     servo6.attach(11);//定义舵机6控制口
  340.     servo7.attach(9);//定义舵机7控制口
  341.     servo8.attach(10);//定义舵机8控制口
  342.     Serial.begin(9600);//串口波特率设置为9600 bps
  343.     Init_Steer();
  344. }
  345. /*
  346. *********************************************************************************************************
  347. ** 函数名称 ：loop()
  348. ** 函数功能 ：主函数
  349. ** 入口参数 ：无
  350. ** 出口参数 ：无
  351. *********************************************************************************************************
  352. */
  353. void Cruising_Mod()//模式功能切换函数
  354.     {
  355.        
  356.      if(Pre_Cruising_Flag != Cruising_Flag)
  357.      {
  358.          if(Pre_Cruising_Flag != 0)
  359.          {
  360.              MOTOR_GO_STOP;
  361.          }
  362.  
  363.          Pre_Cruising_Flag =  Cruising_Flag;
  364.      } 
  365.     switch(Cruising_Flag)
  366.       {
  367.        
  368.        case 2:FollowLine(); return;//巡线模式
  369.        case 3:Avoiding(); return;//避障模式
  370.        case 4:Avoid_wave();return;//超声波避障模式
  371.            case 5:Send_Distance();//超声波距离PC端显示
  372.        default:return;
  373.       }
  374.              
  375. }
  376.  
  377. void loop()
  378.   {  
  379.     while(1)
  380.     {
  381.         Get_uartdata();
  382.         UartTimeoutCheck();
  383.         Cruising_Mod();
  384.      }  
  385.   }
  386.  
  387.  
  388.  
  389. /*
  390. *********************************************************************************************************
  391. ** 函数名称 ：Communication_Decode()
  392. ** 函数功能 ：串口命令解码
  393. ** 入口参数 ：无
  394. ** 出口参数 ：无
  395. *********************************************************************************************************
  396. */
  397. void Communication_Decode()
  398. {  
  399.     if(buffer[0]==0x00)
  400.     {
  401.         switch(buffer[1])   //电机命令
  402.         {
  403.             case 0x01:MOTOR_GO_FORWARD; return;
  404.         case 0x02:MOTOR_GO_BACK;    return;
  405.         case 0x03:MOTOR_GO_LEFT;    return;
  406.             case 0x04:MOTOR_GO_RIGHT;   return;
  407.         case 0x00:MOTOR_GO_STOP;    return;
  408.            default: return;
  409.         }  
  410.     }
  411.    else if(buffer[0]==0x01)//舵机命令
  412.     {
  413.         if(buffer[2]<1)return;
  414.         switch(buffer[1])
  415.         {
  416.            // case 0x01:angle1 = buffer[2];servo1.write(angle1);return;
  417.            // case 0x02:angle2 = buffer[2];servo2.write(angle2);return;
  418.             case 0x01:if(buffer[2]>170)return;angle3 = buffer[2];servo3.write(angle3);return;
  419.             case 0x02:if(buffer[2]>170)return;angle4 = buffer[2];servo4.write(angle4);return;
  420.             case 0x03:if(buffer[2]>170)return;angle5 = buffer[2];servo5.write(angle5);return;
  421.             case 0x04:if((buffer[2]<105)||(buffer[2]>178))return;angle6 = buffer[2];servo6.write(angle6);return;
  422.             case 0x07:if(buffer[2]>170)return;angle7 = buffer[2];servo7.write(angle7);return;
  423.             case 0x08:if(buffer[2]>170)return;angle8 = buffer[2];servo8.write(angle8);return;
  424.             default:return;
  425.         }
  426.     }
  427.    
  428.    else if(buffer[0]==0x02)//调速
  429.     {
  430.                 int i,j;
  431.         if(buffer[2]>10)return;
  432.              
  433.         if(buffer[1]==0x01)//左侧调档
  434.         {
  435.                         i=buffer[2];
  436.             Left_Speed_Hold=Left_Speed[i] ;
  437.                         analogWrite(ENB,Left_Speed_Hold);
  438.         }
  439.                 if(buffer[1]==0x02)//右侧调档
  440.                 {
  441.                         j=buffer[2];
  442.                         Right_Speed_Hold=Right_Speed[j] ;
  443.                         analogWrite(ENA,Right_Speed_Hold);
  444.                 }else return;
  445.         }
  446.     else if(buffer[0]==0x33)//读取舵机角度并赋值
  447.     {
  448.          Init_Steer();return;
  449.         }
  450.     else if(buffer[0]==0x32)//保存命令
  451.     {
  452.        // EEPROM.write(0x01,angle1);
  453.        // EEPROM.write(0x02,angle2);
  454.         EEPROM.write(0x03,angle3);
  455.         EEPROM.write(0x04,angle4);
  456.         EEPROM.write(0x05,angle5);
  457.         EEPROM.write(0x06,angle6);
  458.         EEPROM.write(0x07,angle7);
  459.         EEPROM.write(0x08,angle8);
  460.         return;
  461.     }
  462.         else if(buffer[0]==0x13)//模式切换开关
  463.     {
  464.         switch(buffer[1])
  465.         {
  466.                  
  467.                   case 0x02: Cruising_Flag = 2; return;//巡线
  468.           case 0x03: Cruising_Flag = 3; return;//避障
  469.           case 0x04: Cruising_Flag = 4; return;//雷达避障
  470.                   case 0x05: Cruising_Flag = 5; return;//超声波距离PC端显示
  471.                   case 0x00: Cruising_Flag = 0; return;//正常模式
  472.           default:Cruising_Flag = 0; return;//正常模式
  473.         }
  474.     }
  475.         else if(buffer[0]==0x05)
  476.     {
  477.         switch(buffer[1])   //
  478.         {
  479.             case 0x00:Open_Light(); return;
  480.         case 0x02:Close_Light(); return;
  481.             default: return;
  482.         }  
  483.     }
  484.         else if(buffer[0]==0x40)//存储电机标志
  485.     {
  486.        num=buffer[1];
  487.        EEPROM.write(0x10,num);
  488.     }
  489. }
  490. /*
  491. *********************************************************************************************************
  492. ** 函数名称 ：Get_uartdata()
  493. ** 函数功能 ：读取串口命令
  494. ** 入口参数 ：无
  495. ** 出口参数 ：无
  496. *********************************************************************************************************
  497. */
  498. void Get_uartdata(void)
  499. {
  500.     static int i;
  501.    
  502.     if (Serial.available() > 0) //判断串口缓冲器是否有数据装入
  503.     {
  504.         serial_data = Serial.read();//读取串口
  505.         if(rec_flag==0)
  506.         {
  507.             if(serial_data==0xff)
  508.             {
  509.                 rec_flag = 1;
  510.                 i = 0;
  511.                Costtime = 0;
  512.             }
  513.         }
  514.         else
  515.         {
  516.             if(serial_data==0xff)
  517.             {
  518.                 rec_flag = 0;
  519.                 if(i==3)
  520.                 {
  521.                     Communication_Decode();
  522.                 }
  523.                 i = 0;
  524.             }
  525.             else
  526.             {
  527.                 buffer[i]=serial_data;
  528.                 i++;
  529.             }
  530.         }
  531.     }
  532. }
  533. /*
  534. *********************************************************************************************************
  535. ** 函数名称 ：UartTimeoutCheck()
  536. ** 函数功能 ：串口超时检测
  537. ** 入口参数 ：无
  538. ** 出口参数 ：无
  539. *********************************************************************************************************
  540. */
  541. void UartTimeoutCheck(void)
  542. {
  543.     if(rec_flag == 1)
  544.     {
  545.        Costtime++;  
  546.       if(Costtime == 100000)
  547.       {
  548.            rec_flag = 0;
  549.       }
  550.     }
  551. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!