/*
* 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;
      }
    }
}