View difference between Paste ID: AjJcYct0 and HyFxGsk1

 /*
 * Copyright (c) 2015,小二极客科技有限公司
 * All rights reserved.
 *
 * 文件名称：wifi-robots
 * 文件标识：
 * 摘    要：wifi机器人智能小车控制
 *
 * 当前版本：1.0
 * 作    者：小R团队
 * 完成日期：2015年4月1日
 */
 #include <Servo.h>
 //#include <MsTimer2.h>
 #include <EEPROM.h>
 
 int ledpin = 13;//设置系统启动指示灯
 int ENA = 5;//L298使能A
 int ENB = 6;//L298使能B
 int INPUT2 = 7;//电机接口1
 int INPUT1 = 8;//电机接口2
 int INPUT3 = 12;//电机接口3
 int INPUT4 = 13;//电机接口4
 int num;//定义电机标志位
 
 int Echo = A5;  // 定义超音波信号接收脚位
 int Trig = A4;  // 定义超音波信号发射脚位
 int Input_Detect_LEFT = A3;    //定义小车左侧红外
 int Input_Detect_RIGHT = A2;  //定义小车右侧红外
 int Input_Detect = A1;//定义小车前方红外
 int Carled = A0;//定义小车车灯接口
 int Cruising_Flag = 0;
 int Pre_Cruising_Flag = 0 ;
 int Left_Speed_Hold = 200;//定义左侧速度变量
 int Right_Speed_Hold = 200;//定义右侧速度变量
 
 
 #define MOTOR_GO_FORWARD  {digitalWrite(INPUT1,LOW);digitalWrite(INPUT2,HIGH);digitalWrite(INPUT3,LOW);digitalWrite(INPUT4,HIGH);}    //车体前进
 #define MOTOR_GO_BACK	  {digitalWrite(INPUT1,HIGH);digitalWrite(INPUT2,LOW);digitalWrite(INPUT3,HIGH);digitalWrite(INPUT4,LOW);}    //车体前进
 #define MOTOR_GO_RIGHT	  {digitalWrite(INPUT1,HIGH);digitalWrite(INPUT2,LOW);digitalWrite(INPUT3,LOW);digitalWrite(INPUT4,HIGH);}    //车体前进
 #define MOTOR_GO_LEFT	  {digitalWrite(INPUT1,LOW);digitalWrite(INPUT2,HIGH);digitalWrite(INPUT3,HIGH);digitalWrite(INPUT4,LOW);}    //车体前进
 #define MOTOR_GO_STOP	  {digitalWrite(INPUT1,LOW);digitalWrite(INPUT2,LOW);digitalWrite(INPUT3,LOW);digitalWrite(INPUT4,LOW);}    //车体前进
 
 
 void forward(int num)
 {
 	switch(num)
 	{
 		case 1:MOTOR_GO_FORWARD;return;
 		case 2:MOTOR_GO_FORWARD;return;
 		case 3:MOTOR_GO_BACK;return;
 		case 4:MOTOR_GO_BACK;return;
 		case 5:MOTOR_GO_LEFT;return;
 		case 6:MOTOR_GO_LEFT;return;
 		case 7:MOTOR_GO_RIGHT;return;
 		case 8:MOTOR_GO_RIGHT;return;
 		default:return;
 	}
 }
 
 void back(int num)
 {
 		switch(num)
 	{
 		case 1:MOTOR_GO_BACK;return;
 		case 2:MOTOR_GO_BACK;return;
 		case 3:MOTOR_GO_FORWARD;return;
 		case 4:MOTOR_GO_FORWARD;return;
 		case 5:MOTOR_GO_RIGHT;return;
 		case 6:MOTOR_GO_RIGHT;return;
 		case 7:MOTOR_GO_LEFT;return;
 		case 8:MOTOR_GO_LEFT;return;
 		default:return;
 	}
 }
 void left(int num)
 {
 		switch(num)
 	{
 		case 1:MOTOR_GO_LEFT;return;
 		case 2:MOTOR_GO_RIGHT;return;
 		case 3:MOTOR_GO_LEFT;return;
 		case 4:MOTOR_GO_RIGHT;return;
 		case 5:MOTOR_GO_FORWARD;return;
 		case 6:MOTOR_GO_BACK;return;
 		case 7:MOTOR_GO_FORWARD;return;
 		case 8:MOTOR_GO_BACK;return;
 		default:return;
 	}
 }
 void right(int num)
 {
 		switch(num)
 	{
 		case 1:MOTOR_GO_RIGHT;return;
 		case 2:MOTOR_GO_LEFT;return;
 		case 3:MOTOR_GO_RIGHT;return;
 		case 4:MOTOR_GO_LEFT;return;
 		case 5:MOTOR_GO_BACK;return;
 		case 6:MOTOR_GO_FORWARD;return;
 		case 7:MOTOR_GO_BACK;return;
 		case 8:MOTOR_GO_FORWARD;return;
 		default:return;
 	}
 }
 
 
 int Left_Speed[11]={90,106,122,138,154,170,186,203,218,234,255};//左侧速度档位值
 int Right_Speed[11]={90,106,122,138,154,170,186,203,218,234,255};//右侧速度档位值
 
 //Servo servo1;// 创建舵机#1号
 //Servo servo2;// 创建舵机#2号
 Servo servo3;// 创建舵机#3号
 Servo servo4;// 创建舵机#4号
 Servo servo5;// 创建舵机#5号
 Servo servo6;// 创建舵机#6号
 Servo servo7;// 创建舵机#7号
 Servo servo8;// 创建舵机#8号
 
 
 
 //byte angle1=60;//舵机#1初始值
 //byte angle2=60;//舵机#2初始值
 byte angle3=60;//舵机#3初始值
 byte angle4=60;//舵机#4初始值
 byte angle5=60;//舵机#5初始值
 byte angle6=120;//舵机#6初始值
 byte angle7=60;//舵机#7初始值
 byte angle8=60;//舵机#8初始值
 
 
 int buffer[3];  //串口接收数据缓存
 int rec_flag;   //串口接收标志位
 int serial_data;
 int Uartcount;
 int IR_R;
 int IR_L;
 int IR;
 unsigned long Pretime;
 unsigned long Nowtime;
 unsigned long Costtime;
 float Ldistance;
 
 void Open_Light()//开大灯
     {
       digitalWrite(Carled,HIGH);   //拉低电平，正极接电源，负极接Io口
       delay(1000);
     }
 void Close_Light()//关大灯
     {
       digitalWrite(Carled, LOW);   //拉低电平，正极接电源，负极接Io口
       delay(1000);
     }
 
 void  Avoiding()//红外避障函数
     {
       IR = digitalRead(Input_Detect);
        if((IR == HIGH))
        {
           forward(num);;//直行
           return;
        }
        if((IR == LOW))
        {
             MOTOR_GO_STOP;//停止
             return;
        }
     }
 
 
 
 void FollowLine()   // 巡线模式
     {
       IR_L = digitalRead(Input_Detect_LEFT);//读取左边传感器数值
       IR_R = digitalRead(Input_Detect_RIGHT);//读取右边传感器数值
 
       if((IR_L == LOW) && (IR_R == LOW))//两边同时探测到障碍物
       {
         forward(num);//直行
         return;
 
       }
       if((IR_L == LOW) && (IR_R == HIGH))//右侧遇到障碍
       {
         left(num);//左转
         return;
 
       }
       if((IR_L == HIGH) &&( IR_R == LOW))//左侧遇到障碍
       {
         right(num);//右转
         return;
 
       }
       if((IR_L == HIGH) && (IR_R == HIGH))//左右都检测到，就如视频中的那样遇到一道横的胶带
       {
         MOTOR_GO_STOP;//停止
         return;
        }
     }
 
 char Get_Distence()//测出距离
  {
       digitalWrite(Trig, LOW);   // 让超声波发射低电压2μs
       delayMicroseconds(2);
       digitalWrite(Trig, HIGH);  // 让超声波发射高电压10μs，这里至少是10μs
       delayMicroseconds(10);
       digitalWrite(Trig, LOW);    // 维持超声波发射低电压
       Ldistance = pulseIn(Echo, HIGH);  // 读差相差时间
       Ldistance= Ldistance/5.8/10;      // 将时间转为距离距离（单位：公分）
     //  Serial.println(Ldistance);      //显示距离
       return Ldistance;
 
   }
 
 void Avoid_wave()//超声波避障函数
 {
   Get_Distence();
   if(Ldistance < 15)
       {
           MOTOR_GO_STOP;
       }
       else
       {
            forward(num);
       }
 }
 
 void Avoid_wave_auto()
 {
   Get_Distence();
   if(Ldistance < 15)
   {
     back(num);;
     delay(300);
     MOTOR_GO_STOP;
   }
 }
 
 void Send_Distance()//超声波距离PC端显示
 {
   int dis= Get_Distence();
   Serial.write(0xff);
   Serial.write(0x03);
   Serial.write(0x00);
   Serial.write(dis);
   Serial.write(0xff);
   delay(1000);
 }
 /*
 *********************************************************************************************************
 ** 函数名称 ：Delayed()
 ** 函数功能 ：延时程序
 ** 入口参数 ：无
 ** 出口参数 ：无
 *********************************************************************************************************
 */
 void  Delayed()    //延迟40秒等WIFI模块启动完毕
 {
     int i;
     for(i=0;i<20;i++)
     {
         digitalWrite(ledpin,LOW);
         delay(1000);
         digitalWrite(ledpin,HIGH);
         delay(1000);
     }
 }
 
 /*
 *********************************************************************************************************
 ** 函数名称 ：setup().Init_Steer()
 ** 函数功能 ：系统初始化（串口、电机、舵机、指示灯初始化）。
 ** 入口参数 ：无
 ** 出口参数 ：无
 *********************************************************************************************************
 */
 void Init_Steer()//舵机初始化(角度为上次保存数值)
 {
    // angle1 = EEPROM.read(0x01);//读取寄存器0x01里面的值
    // angle2 = EEPROM.read(0x02);//读取寄存器0x02里面的值
     angle3 = EEPROM.read(0x03);//读取寄存器0x03里面的值
     angle4 = EEPROM.read(0x04);//读取寄存器0x04里面的值
     angle5 = EEPROM.read(0x05);//读取寄存器0x05里面的值
     angle6 = EEPROM.read(0x06);//读取寄存器0x06里面的值
     angle7 = EEPROM.read(0x07);//读取寄存器0x07里面的值
     angle8 = EEPROM.read(0x08);//读取寄存器0x08里面的值
 
     if(angle7 == 255 && angle8 == 255)
     {
        // EEPROM.write(0x01,60);//把初始角度存入地址0x01里面
        // EEPROM.write(0x02,60);//把初始角度存入地址0x02里面
         EEPROM.write(0x03,60);//把初始角度存入地址0x03里面
         EEPROM.write(0x04,60);//把初始角度存入地址0x04里面
         EEPROM.write(0x05,60);//把初始角度存入地址0x05里面
         EEPROM.write(0x06,120);//把初始角度存入地址0x06里面
         EEPROM.write(0x07,60);//把初始角度存入地址0x07里面
         EEPROM.write(0x08,60);//把初始角度存入地址0x08里面
         return;
     }
 
    // servo1.write(angle1);//把保存角度赋值给舵机1
    // servo2.write(angle2);//把保存角度赋值给舵机2
     servo3.write(angle3);//把保存角度赋值给舵机3
     servo4.write(angle4);//把保存角度赋值给舵机4
     servo5.write(angle5);//把保存角度赋值给舵机5
     servo6.write(angle6);//把保存角度赋值给舵机6
     servo7.write(angle7);//把保存角度赋值给舵机7
     servo8.write(angle8);//把保存角度赋值给舵机8
     num = EEPROM.read(0x10);//读取寄存器0x10里面的值
     if(num==0xff)EEPROM.write(0x10,1);
 }
 
 void setup()
 {
     pinMode(ledpin,OUTPUT);
     pinMode(ENA,OUTPUT);
     pinMode(ENB,OUTPUT);
     pinMode(INPUT1,OUTPUT);
     pinMode(INPUT2,OUTPUT);
     pinMode(INPUT3,OUTPUT);
     pinMode(INPUT4,OUTPUT);
     pinMode(Input_Detect_LEFT,INPUT);
     pinMode(Input_Detect_RIGHT,INPUT);
     pinMode(Carled, OUTPUT);
     pinMode(Input_Detect,INPUT);
     pinMode(Echo,INPUT);
     pinMode(Trig,OUTPUT);
 
     Delayed();//延迟40秒等WIFI模块启动完毕
     analogWrite(ENB,Left_Speed_Hold);//给L298使能端B赋值
     analogWrite(ENA,Right_Speed_Hold);//给L298使能端A赋值
     digitalWrite(ledpin,LOW);
     //servo1.attach(SDA);//定义舵机1控制口
     //servo2.attach(SCL);//定义舵机2控制口
     servo3.attach(3);//定义舵机3控制口
     servo4.attach(4);//定义舵机4控制口
     servo5.attach(2);//定义舵机5控制口
     servo6.attach(11);//定义舵机6控制口
     servo7.attach(9);//定义舵机7控制口
     servo8.attach(10);//定义舵机8控制口
     Serial.begin(9600);//串口波特率设置为9600 bps
     Init_Steer();
 }
 /*
 *********************************************************************************************************
 ** 函数名称 ：loop()
 ** 函数功能 ：主函数
 ** 入口参数 ：无
 ** 出口参数 ：无
 *********************************************************************************************************
 */
 void Cruising_Mod()//模式功能切换函数
     {
 
 	 if(Pre_Cruising_Flag != Cruising_Flag)
 	 {
 	     if(Pre_Cruising_Flag != 0)
 		 {
 		     MOTOR_GO_STOP;
 		 }
 
     	 Pre_Cruising_Flag =  Cruising_Flag;
 	 }
 	switch(Cruising_Flag)
 	  {
 
 	   case 2:FollowLine(); return;//巡线模式
 	   case 3:Avoiding(); return;//避障模式
 	   case 4:Avoid_wave();return;//超声波避障模式
            case 5:Send_Distance();//超声波距离PC端显示
 	   default:return;
 	  }
 
 }
 
 void loop()
   {
     while(1)
     {
         Get_uartdata();
         UartTimeoutCheck();
         Cruising_Mod();
      }
   }
 
 
 
 /*
 *********************************************************************************************************
 ** 函数名称 ：Communication_Decode()
 ** 函数功能 ：串口命令解码
 ** 入口参数 ：无
 ** 出口参数 ：无
 *********************************************************************************************************
 */
 void Communication_Decode()
 {
     if(buffer[0]==0x00)
     {
         switch(buffer[1])   //电机命令
         {
             case 0x01:MOTOR_GO_FORWARD; return;
 	    case 0x02:MOTOR_GO_BACK;    return;
 	    case 0x03:MOTOR_GO_LEFT;    return;
             case 0x04:MOTOR_GO_RIGHT;   return;
 	    case 0x00:MOTOR_GO_STOP;    return;
            default: return;
         }
     }
    else if(buffer[0]==0x01)//舵机命令
     {
         if(buffer[2]<1)return;
         switch(buffer[1])
         {
            // case 0x01:angle1 = buffer[2];servo1.write(angle1);return;
            // case 0x02:angle2 = buffer[2];servo2.write(angle2);return;
             case 0x01:if(buffer[2]>170)return;angle3 = buffer[2];servo3.write(angle3);return;
             case 0x02:if(buffer[2]>170)return;angle4 = buffer[2];servo4.write(angle4);return;
             case 0x03:if(buffer[2]>170)return;angle5 = buffer[2];servo5.write(angle5);return;
             case 0x04:if((buffer[2]<105)||(buffer[2]>178))return;angle6 = buffer[2];servo6.write(angle6);return;
             case 0x07:if(buffer[2]>170)return;angle7 = buffer[2];servo7.write(angle7);return;
             case 0x08:if(buffer[2]>170)return;angle8 = buffer[2];servo8.write(angle8);return;
             default:return;
         }
     }
 
    else if(buffer[0]==0x02)//调速
 	{
                 int i,j;
 		if(buffer[2]>10)return;
 
 		if(buffer[1]==0x01)//左侧调档
 		{
                         i=buffer[2];
 			Left_Speed_Hold=Left_Speed[i] ;
                         analogWrite(ENB,Left_Speed_Hold);
 		}
                 if(buffer[1]==0x02)//右侧调档
                 {
                         j=buffer[2];
                         Right_Speed_Hold=Right_Speed[j] ;
                         analogWrite(ENA,Right_Speed_Hold);
                 }else return;
         }
     else if(buffer[0]==0x33)//读取舵机角度并赋值
 	{
 		 Init_Steer();return;
         }
     else if(buffer[0]==0x32)//保存命令
     {
        // EEPROM.write(0x01,angle1);
        // EEPROM.write(0x02,angle2);
         EEPROM.write(0x03,angle3);
         EEPROM.write(0x04,angle4);
         EEPROM.write(0x05,angle5);
         EEPROM.write(0x06,angle6);
         EEPROM.write(0x07,angle7);
         EEPROM.write(0x08,angle8);
         return;
     }
     	else if(buffer[0]==0x13)//模式切换开关
 	{
 	    switch(buffer[1])
 		{
 
                   case 0x02: Cruising_Flag = 2; return;//巡线
 		  case 0x03: Cruising_Flag = 3; return;//避障
 		  case 0x04: Cruising_Flag = 4; return;//雷达避障
                   case 0x05: Cruising_Flag = 5; return;//超声波距离PC端显示
                   case 0x00: Cruising_Flag = 0; return;//正常模式
 		  default:Cruising_Flag = 0; return;//正常模式
 		}
 	}
         else if(buffer[0]==0x05)
     {
         switch(buffer[1])   //
         {
             case 0x00:Open_Light(); return;
 	    case 0x02:Close_Light(); return;
             default: return;
         }
     }
         else if(buffer[0]==0x40)//存储电机标志
 	{
 	   num=buffer[1];
 	   EEPROM.write(0x10,num);
 	}
 }
 /*
 *********************************************************************************************************
 ** 函数名称 ：Get_uartdata()
 ** 函数功能 ：读取串口命令
 ** 入口参数 ：无
 ** 出口参数 ：无
 *********************************************************************************************************
 */
 void Get_uartdata(void)
 {
     static int i;
 
     if (Serial.available() > 0) //判断串口缓冲器是否有数据装入
     {
         serial_data = Serial.read();//读取串口
         if(rec_flag==0)
         {
             if(serial_data==0xff)
             {
                 rec_flag = 1;
                 i = 0;
                Costtime = 0;
             }
         }
         else
         {
             if(serial_data==0xff)
             {
                 rec_flag = 0;
                 if(i==3)
                 {
                     Communication_Decode();
                 }
                 i = 0;
             }
             else
             {
                 buffer[i]=serial_data;
                 i++;
             }
         }
     }
 }
 /*
 *********************************************************************************************************
 ** 函数名称 ：UartTimeoutCheck()
 ** 函数功能 ：串口超时检测
 ** 入口参数 ：无
 ** 出口参数 ：无
 *********************************************************************************************************
 */
 void UartTimeoutCheck(void)
 {
     if(rec_flag == 1)
     {
        Costtime++;
       if(Costtime == 100000)
       {
            rec_flag = 0;
       }
     }
 }