//WiFi control of the sketch. by CNC Paradise. (july-2016)
#include "ESP8266WiFi.h"
#include "WiFiUDP.h"
#include "Servo.h"
Servo leftservo;
Servo rightservo;

WiFiUDP UDP;
char recbuff[2];
IPAddress myIP(192, 168, 4, 1);
IPAddress HOSTIP (192, 168, 4, 2);
const char *ssid = "TJ-8"; // ID is the same configuration as the controller side.
const char *password = "*********"; // password is the same configuration as the controller and the side.

int IRsensor_L;
int IRsensor_R;
int IRsensor_FL;
int IRsensor_FR;
int ModeSW;
int Val_x, Val_y, Val_z, Val_c ;
int x, y, z, c;
const int turntime = 400; // 自動運転時旋回時間の設定

void setup() {
  WiFi.softAP(ssid, password); // setting SSID and Password
  WiFi.config(myIP, WiFi.gatewayIP(), WiFi.subnetMask()); // setting IP Address
  UDP.begin(8000); // setting UDP port,port number is the same setting as the controller and the side.
  Serial.begin(115200);

  pinMode(12, INPUT); // L_IRsensor
  pinMode(13, INPUT); // FL_IRsensor
  pinMode(14, INPUT); // FR_IRsensor
  pinMode(16, INPUT); // R_IRsensor
  pinMode(2, OUTPUT); // WiFi Status LED
  pinMode(15,OUTPUT); // IRsensor-Power_Control
  digitalWrite(12, HIGH);
  digitalWrite(13, HIGH);
  digitalWrite(14, HIGH);
  digitalWrite(16, HIGH);
  digitalWrite(15,  LOW);
  ModeSW = 1;
}

 // 以下のサーボPWM値設定は機体により要調整（スピードやモーターの負担を考慮）
void Forwards() { // 前進時のサーボPWM値設定（固定）
  leftservo.write(30);
  rightservo.write(150);
}

void backwards() { // 後進時のサーボPWM値設定（固定）
  leftservo.write(150);
  rightservo.write(30);
}

void turnleft(int t) { // 左旋回時のサーボPWM値設定（固定）
  leftservo.write(150);
  rightservo.write(150);
  delay(t);
}

void turnright(int t) { // 右旋回時のサーボPWM値設定（固定）
  leftservo.write(30);
  rightservo.write(30);
  delay(t);
}

void stopmove() { // 停止時のサーボPWM値設定（固定）
  leftservo.write(90);
  rightservo.write(90);
}

void Automatic_Control() { // IRセンサーによる自動運転の処理
  IRsensor_L  = digitalRead(12);
  IRsensor_R  = digitalRead(13);
  IRsensor_FL = digitalRead(16);
  IRsensor_FR = digitalRead(14);
  stopmove();
  Forwards();
  if (IRsensor_L == 0 ) {
    turnright(turntime);
    Forwards();
  }
  if (IRsensor_R == 0 ) {
    turnleft(turntime);
    Forwards();
  }
  if (IRsensor_FL == 0 ) {
    stopmove();
    delay(50);
    backwards();
    delay(200);
    turnleft(turntime);
    Forwards();
  }
  if (IRsensor_FR == 0 ) {
    stopmove();
    delay(50);
    backwards();
    delay(200);
    turnleft(turntime);
    Forwards();
  }
}

void Manual_Control() { // 手動運転の処理　0～255の受信入力値をPWM値に設定
    if (x <= 100) { // turnleft　左旋回
      leftservo.write(150);
      rightservo.write(150);
    }
    if (x >= 155) { // turnleft　右旋回
      leftservo.write(30);
      rightservo.write(30);
    }
    if (y >= 155) { // 前進
      Forwards();
    }
    if (y <= 100) { // 後退
      backwards();
    }
    if (y >= 101 & y <= 154 & x >= 101 & x <= 154) { // 停止範囲の設定
      stopmove();
    }  
}
  
void loop() {
  if (UDP.parsePacket() > 0) { // UDPパケットの受信処理
    UDP.read(recbuff, 2);
    UDP.flush();
  }
  switch (recbuff[0]) {
    case 'X':
      x = recbuff[1];
      break;
    case 'Y':
      y = recbuff[1];
      break;
    case 'Z':
      z = recbuff[1];
      if (z == 0) {
        ModeSW = 0;
      }
      break;
    case 'C':
      c = recbuff[1];
      if (c == 0) {
        ModeSW = 1;
      }
      break;
  }

  if (ModeSW == 0) { // Automatic_Mode　自動モード
    Automatic_Control();
    digitalWrite(15, HIGH ); // IRsensor-Power_ON
  }
  if (ModeSW == 1) { // Manual_Mode　手動操作モード
    Manual_Control();
    digitalWrite(15, LOW ); // IRsensor-Power_OFF
  }

  if (WiFi.status() != WL_CONNECTED) {
    digitalWrite(2, LOW); // WiFi Status LED_OFF
    stopmove();
    leftservo.detach();
    rightservo.detach();
  } else {
    digitalWrite(2, HIGH ); // WiFi Status LED_ON
  leftservo.attach(4);
  rightservo.attach(5);    
  }
}