#include <OneWire.h>

#define LIGHT_PIN    A2 //光敏模块
#define FLAME_PIN    A3 //火焰模块

#define TEMP_PIN     2 //DS18B20
OneWire ds(TEMP_PIN); 

#define MOTION_PIN   4 //人体感应模块

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

#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, 
}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(LIGHT_PIN);
  Serial.print("Lightsensor:");
  if(light<=200)
   Serial.println("Brightness");      //能见度高
  else if(light>200&&light<600)
    Serial.println("Medium");           //能见度一般
  else Serial.println("Darkness");  //能见度低
}

/*获取火焰传感器值*/
void Get_FlameSenor()
{
  int flame_val;
  flame_val=analogRead(FLAME_PIN);
  Serial.print("FlaeSensor:");
  Serial.println(flame_val);
}

/*检测是否有人*/
void Get_MotionSensor()
{
  int val=digitalRead(MOTION_PIN);
  
  if(val==HIGH)
    Serial.println("MotionSensor:People!"); 
   else
    Serial.println("MotionSensor:No People!");  
}

//电机控制
void go_Advance()  //双轮前进
{
  if(motor_update_flag ==1) return;
  motor_update_flag = 1;

  analogWrite(MOTOR_R1_PIN, 150);
  analogWrite(MOTOR_R2_PIN, 0);
  analogWrite(MOTOR_L1_PIN, 150);
  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, 150);
  analogWrite(MOTOR_L1_PIN, 150);
  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, 150);
  analogWrite(MOTOR_R2_PIN, 0);
  analogWrite(MOTOR_L1_PIN, 0);
  analogWrite(MOTOR_L2_PIN, 150);
  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,150);
  analogWrite(MOTOR_L1_PIN, 0);
  analogWrite(MOTOR_L2_PIN, 150);
  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, 150);
  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, 150);
}

//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;
  }
  /*
  if(Serial.available()>0)
  {
      char temp[32];
      memset(temp,0x00,32);
      size_t len=Serial.available();
      if(len<32) Serial.readBytes(temp,len);
      if(temp[0]=='C') {
        Serial.println(temp);
        Serial.println("You have modified the parameters!");
        sscanf(temp,"CMD%d,%d,%d",&mindistn1,&mindistn2,&mindistn3);
      }
      else if(len<=1&&temp!=NULL) Uart_Date=temp[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;Serial.println("car STOP !");break;
    case '1':Drive_Status=AUTO_DRIVE_AG; Serial.println("Start_AUTO_AG...");break;
    default:break;
  }
}


//小车电机控制
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();Read_DS18B20();Get_Photo();Get_FlameSenor();Get_MotionSensor();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();
      }
    }
  }
}
//初始化
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(MOTION_PIN,INPUT);
}
//主函数
void setup()
{
  Serial.begin(9600);//为适配蓝牙模块的默认波特率，只能9600
  init_GPIO();
}

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