#include <Servo.h>
#include <OneWire.h>
/*点击APP上的暂停按钮获取光照值和温度值*/
int inPin=2; // define D2 as input pin connecting to DS18S20 S pin
OneWire ds(inPin); 

#define SERVO_PIN     9
Servo myservo;

#define UIM_OUT_PIN   12 // 定义超声波信号发射脚位tx
#define UIM_IN_PIN    8  // 定义超声波信号接收脚位rx

#define IRM_L_PIN     A0//左红外
#define IRM_R_PIN     A1//右红外

int lightPin = A2;  //define a pin for Photoresistor

#define MOTOR_R1_PIN  5//右
#define MOTOR_R2_PIN  6//右
#define MOTOR_L1_PIN  3//左
#define MOTOR_L2_PIN  11//左

#define BUZZ_PIN    13

#define AG_L_PIN    7
#define AG_M_PIN    4
#define AG_R_PIN    10

#define UIM_ANGLE_F 83      //正前方
#define UIM_ANGLE_R 5       //右方
#define UIM_ANGLE_L 177     //左方

#define MAX_PACKETSIZE 32    //串口接收缓冲区
char buffUART[MAX_PACKETSIZE];
unsigned int buffUARTIndex = 0;
unsigned long preUARTTick = 0;


enum DS
{
  MANUAL_DRIVE,
  AUTO_DRIVE_AG, 
  AUTO_DRIVE_UI
}Drive_Status=MANUAL_DRIVE;

enum DN
{ 
  GO_ADVANCE, 
  GO_LEFT, 
  GO_RIGHT,
  GO_BACK,
  STOP_STOP,
  DEF
}Drive_Num=DEF;


int mindistn1=10;
int mindistn2=30;
int mindistn3=20;

bool flag1=false;
bool stopFlag = true;
bool JogFlag = false;
uint16_t JogTimeCnt = 0;
uint32_t JogTime=0;

uint8_t motor_update_flag = 0;

/*读取DS18B20温度值*/
void Read_DS18B20()
{
  int HighByte, LowByte, TReading, SignBit, Tc_100, Whole, Fract;
  byte i;
  byte present = 0;
  byte data[12];
  byte addr[8];
 
  if ( !ds.search(addr)) {
      ds.reset_search();
      return;
  }
 
 
  ds.reset();
  ds.select(addr);
  ds.write(0x44,1); 
 
  delay(1000);  
 
 
  present = ds.reset();
  ds.select(addr);    
  ds.write(0xBE);  
 
 
 
  for ( i = 0; i < 9; i++) { 
    data[i] = ds.read();
  }
  Serial.print("Temperature: ");
  LowByte = data[0];
  HighByte = data[1];
  TReading = (HighByte << 8) + LowByte;
  SignBit = TReading & 0x8000;  
  if (SignBit)
  {
    TReading = (TReading ^ 0xffff) + 1;
  }
  Tc_100 = (6 * TReading) + TReading / 4; 
  Whole = Tc_100 / 100; 
  Fract = Tc_100 % 100;
  if (SignBit)
  {
     Serial.print("-");
  }
  Serial.print(Whole);
  Serial.print(".");
  if (Fract < 10)
  {
     Serial.print("0");
  }
  Serial.print(Fract);
  Serial.print(" C\n");  
}

/*获取环境光照强度*/
void Get_Photo()
{
  int light=0;
  light=analogRead(lightPin);
  Serial.print("Lightsensor:");
  if(light<=200)
   Serial.println("Brightness");      //能见度高
  else if(light>200&&light<600)
    Serial.println("Medium");           //能见度一般
  else Serial.println("Darkness");  //能见度低
  
}
//电机控制
void go_Advance()  //双轮前进
{
  if(motor_update_flag ==1) return;
  motor_update_flag = 1;

  analogWrite(MOTOR_R1_PIN, 170);
  analogWrite(MOTOR_R2_PIN, 0);
  analogWrite(MOTOR_L1_PIN, 170);
  analogWrite(MOTOR_L2_PIN, 0);
}
void go_Left(int t=0)  //双轮左转
{
  if(motor_update_flag ==2) return;
  motor_update_flag = 2;
  
  analogWrite(MOTOR_R1_PIN, 0);
  analogWrite(MOTOR_R2_PIN, 170);
  analogWrite(MOTOR_L1_PIN, 170);
  analogWrite(MOTOR_L2_PIN, 0);
  delay(t);
}
void go_Right(int t=0)  //双轮右转
{
  if(motor_update_flag ==3) return;
  motor_update_flag = 3;
  
  analogWrite(MOTOR_R1_PIN, 170);
  analogWrite(MOTOR_R2_PIN, 0);
  analogWrite(MOTOR_L1_PIN, 0);
  analogWrite(MOTOR_L2_PIN, 170);
  delay(t);
}
void go_Back(int t=0)  //双轮后退
{
  if(motor_update_flag ==4) return;
  motor_update_flag = 4;

  analogWrite(MOTOR_R1_PIN, 0);
  analogWrite(MOTOR_R2_PIN, 170);
  analogWrite(MOTOR_L1_PIN, 0);
  analogWrite(MOTOR_L2_PIN, 170);
  delay(t);
}
void stop_Stop()    //双轮停止
{
  if(motor_update_flag ==5) return;
  motor_update_flag = 5;
  
  analogWrite(MOTOR_R1_PIN, 0);
  analogWrite(MOTOR_R2_PIN, 0);
  analogWrite(MOTOR_L1_PIN, 0);
  analogWrite(MOTOR_L2_PIN, 0);
}
void go_Oleft()   //单轮左转
{
  if(motor_update_flag ==6) return;
  motor_update_flag = 6;
 
  analogWrite(MOTOR_R1_PIN, 0);
  analogWrite(MOTOR_R2_PIN, 170);
  analogWrite(MOTOR_L1_PIN, 0);
  analogWrite(MOTOR_L2_PIN, 0);
}
void go_Oright()   //单轮右转
{
  if(motor_update_flag ==7) return;
  motor_update_flag = 7;
  
  analogWrite(MOTOR_R1_PIN, 0);
  analogWrite(MOTOR_R2_PIN, 0);
  analogWrite(MOTOR_L1_PIN, 0);
  analogWrite(MOTOR_L2_PIN, 170);
}
void buzz_ON()   //蜂鸣器开
{
  if(motor_update_flag ==8) return;
  motor_update_flag = 8;
  digitalWrite(BUZZ_PIN, LOW);
}
void buzz_OFF()  //蜂鸣器关
{
  if(motor_update_flag ==9) return;
  motor_update_flag = 9;
  digitalWrite(BUZZ_PIN, HIGH);
}
//左右红外避障
uint8_t read_IRM()
{
  uint8_t irml = analogRead(IRM_L_PIN)>500?1:0;
  uint8_t irmr = analogRead(IRM_R_PIN)>500?1:0;
  uint8_t temp =0;
  temp |= irml;
  temp = (temp <<1)|irmr; 
  return 0x03&temp;
}
//控制舵机旋转(避免转速过快导致开发板电压被拉得过低而重启)
void servo_control(int angle)
{
  static int last_angle = UIM_ANGLE_F;
  //myservo.write(angle);
  int i=0;
  int diff = angle - last_angle;
  if(diff > 0)
  {
    for(i=0; i<diff; i++)
    {
      myservo.write(last_angle + i);
      delay(5);
    }
  }
  else
  {
    diff = -diff;
    for(i=0; i<diff; i++)
    {
      myservo.write(last_angle - i);
      delay(5);
    }
  }
  last_angle = angle;
}

//超声波测距
int read_UIM(int angle)
{
  servo_control(angle);
  // delay(500);
  digitalWrite(UIM_OUT_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(UIM_OUT_PIN, HIGH); //min>=10μs
  delayMicroseconds(10);
  digitalWrite(UIM_OUT_PIN, LOW);

  // float distance = pulseIn(UIM_IN_PIN, HIGH, 150*1000);//如果加上第三个参数(Timeout超时时间)，则会导致当距离过远而超时返回值为0的BUG
  float distance = pulseIn(UIM_IN_PIN, HIGH);
  distance = distance / 58.0;
  int val =(int)distance;  
  if (angle == UIM_ANGLE_F) Serial.print("Front: ");
  else if (angle == UIM_ANGLE_L) Serial.print("Left: ");
  else if (angle == UIM_ANGLE_R) Serial.print("Right: ");
  Serial.print(val > 400 ? 999 : val); //显示距离
  Serial.println("cm. "); //输出单位

  return val;
}

//WiFi/蓝牙 通过串口控制
void do_Uart_Tick()
{

  char Uart_Date=0;
  if(Serial.available()) 
  {
    size_t len = Serial.available();
    uint8_t sbuf[len + 1];
    sbuf[len] = 0x00;
    Serial.readBytes(sbuf, len);
    //parseUartPackage((char*)sbuf);
    memcpy(buffUART + buffUARTIndex, sbuf, len);//确保串口能读完整一帧数据
    buffUARTIndex += len;
    preUARTTick = millis();
    if(buffUARTIndex >= MAX_PACKETSIZE - 1) 
    {
      buffUARTIndex = MAX_PACKETSIZE - 2;
      preUARTTick = preUARTTick - 200;
    }
  }
  if(buffUARTIndex > 0 && (millis() - preUARTTick >= 100))//在APP发送标志量修改避障参数
  { //data ready
    buffUART[buffUARTIndex] = 0x00;
    //处理
    if(buffUART[0]=='C') 
    {
      Serial.println(buffUART);
      Serial.println("You have modified the parameters!");//提示已经修改避障距离参数
      sscanf(buffUART,"CMD%d,%d,%d",&mindistn1,&mindistn2,&mindistn3);
      // Serial.println(mindistn1);
      // Serial.println(mindistn2);
      // Serial.println(mindistn3);
    }
    else  Uart_Date=buffUART[0];
    buffUARTIndex = 0;
  }
  
  switch (Uart_Date)    //串口控制指令
  {
    case '2':Drive_Status=MANUAL_DRIVE; Drive_Num=GO_ADVANCE;Serial.println("car GO_ADVANCE !"); break;
    case '4':Drive_Status=MANUAL_DRIVE; Drive_Num=GO_LEFT; Serial.println("car GO_LEFT !");break;
    case '6':Drive_Status=MANUAL_DRIVE; Drive_Num=GO_RIGHT; Serial.println("car GO_RIGHT !");break;
    case '8':Drive_Status=MANUAL_DRIVE; Drive_Num=GO_BACK; Serial.println("car GO_BACK !");break;
    case '5':Drive_Status=MANUAL_DRIVE; Drive_Num=STOP_STOP;buzz_OFF(); Read_DS18B20();Get_Photo();Serial.println("car STOP !");break;
    case '3':Drive_Status=AUTO_DRIVE_UI; Serial.println("AUTO_DRIVE_UI...");break;
    case '1':Drive_Status=AUTO_DRIVE_AG; Serial.println("Start_AUTO_AG...");break;
    default:break;
  }
}


//自动循迹
void start_AUTO_AG()
{

  int SL = digitalRead(AG_L_PIN);
  int SM = digitalRead(AG_M_PIN);
  int SR = digitalRead(AG_R_PIN);
  if (SM == HIGH)//中传感器在黑色区域
  {
    if (SL == HIGH && SR == LOW) // 左黑右白, 向左转弯
    {
      go_Oleft();
    }
    else if (SL == LOW && SR == HIGH) //左白右黑, 向右转弯
    {
      go_Oright();
    }
    else if (SL == LOW && SR == LOW) // 两侧均为白色, 直进
    {
      go_Advance();
    }
    else
    {
      go_Oleft();
    }
  }
  else // 中传感器在白色区域
  {
    if (SL == HIGH && SR == LOW)// 左黑右白, 快速左转
    {
      go_Oleft();
    }
    else if (SL == LOW && SR == HIGH) // 左白右黑, 快速右转
    {
      go_Oright();
    }
    else if (SL == HIGH && SR == HIGH) // 左黑右黑, 向左转弯
    {
      go_Oleft();
    }
    else // 都是白色, 停止
    {
      go_Back();
    }
  }
}
//自动避障
void start_ATUO_UI(void)
{
  uint8_t abir = read_IRM();
  int Fspeedd = read_UIM(UIM_ANGLE_F);
  if ((Fspeedd < mindistn1 && Fspeedd!=0) || abir!=0x03) // 假如前方距离小于 mindistn1 (公分)
  {
    stop_Stop();
    buzz_ON();
    go_Back(100);
  }
  else if ((Fspeedd > mindistn1&&Fspeedd < mindistn2  && Fspeedd!=0) || abir != 0x03) // 假如前方距离小于 mindistn2 (公分)
  {
    stop_Stop(); 
    buzz_ON();
    int Lspeedd = read_UIM(UIM_ANGLE_L);
    read_UIM(UIM_ANGLE_F); 
    int Rspeedd = read_UIM(UIM_ANGLE_R);
    if (Lspeedd > Rspeedd) //假如左边距离大于右边距离
    {
      go_Back(100);
      go_Left(600);
    }
    if (Lspeedd <= Rspeedd) //假如左边距离小于或等于右边距离
    {
      go_Back(100);
      go_Right(600); 
    }
    if (Lspeedd < mindistn3 && Rspeedd < mindistn3) //假如左边距离和右边距离皆小于 mindistn3 (公分)
    {
      go_Back(800); 
      go_Left(300); //防卡
    }
  }
  else 
  {
    go_Advance();
    buzz_OFF();
  }
}

//小车电机控制
void do_Drive_Tick()
{
  if(Drive_Status == MANUAL_DRIVE)
  {
    switch (Drive_Num) 
    {
      case GO_ADVANCE:go_Advance();JogFlag = true;JogTimeCnt = 1;JogTime=millis();break;
      case GO_LEFT: go_Left();JogFlag = true;JogTimeCnt = 1;JogTime=millis();break;
      case GO_RIGHT:  go_Right();JogFlag = true;JogTimeCnt = 1;JogTime=millis();break;
      case GO_BACK: go_Back();JogFlag = true;JogTimeCnt = 1;JogTime=millis();break;
      case STOP_STOP: stop_Stop();JogTime = 0;break;
      default:break;
    }
    Drive_Num=DEF;
    //保持小车运动状态100ms
    if(millis()-JogTime>=100)
    {
      JogTime=millis();
      if(JogFlag == true) 
      {
        stopFlag = false;
        if(JogTimeCnt <= 0) 
        {
          JogFlag = false; stopFlag = true;
        }
        JogTimeCnt--;
      }
      if(stopFlag == true) 
      {
        JogTimeCnt=0;
        stop_Stop();
      }
    }
  }
  else if(Drive_Status==AUTO_DRIVE_AG)
  {
    start_AUTO_AG();
  }
  else if(Drive_Status==AUTO_DRIVE_UI)
  {
    start_ATUO_UI();
  }
}
//初始化
void init_GPIO()
{
  pinMode(MOTOR_R1_PIN, OUTPUT); 
  pinMode(MOTOR_R2_PIN, OUTPUT); 
  pinMode(MOTOR_L1_PIN, OUTPUT);  
  pinMode(MOTOR_L2_PIN, OUTPUT);
  stop_Stop();
  pinMode(AG_R_PIN, INPUT); 
  pinMode(AG_M_PIN, INPUT);
  pinMode(AG_L_PIN, INPUT); 

  pinMode(UIM_IN_PIN, INPUT); 
  pinMode(UIM_OUT_PIN, OUTPUT); 

  pinMode(BUZZ_PIN, OUTPUT);
  digitalWrite(BUZZ_PIN, HIGH);   
}
//主函数
void setup()
{
  Serial.begin(9600);//为适配蓝牙模块的默认波特率，只能9600
  init_GPIO();
  myservo.attach(SERVO_PIN); // 定义舵机输出第9脚位(PWM)
}

void loop()
{
  do_Uart_Tick();
  do_Drive_Tick();
 
}