void loop()
{
	if (telnet.connected()) {
		while (telnet.available()) Serial.write(telnet.read());
	} else {
		telnet.begin(&server);
	}
}

void set_Motorspeed(int SPEED_L,int SPEED_R)
{
  analogWrite(PIN_ENB, SPEED_L);
  analogWrite(PIN_ENA, SPEED_R);
}

void go_Advance(void)  //Forward
{
  digitalWrite(PIN_IN1, HIGH); // IN1
  digitalWrite(PIN_IN2, LOW);  // IN2
  digitalWrite(PIN_IN3, HIGH); // IN3
  digitalWrite(PIN_IN4, LOW);  // IN4
  set_Motorspeed(SPEED, SPEED);
}


void loop()
{
	if (Serial.available()) {
		elapsed_time = millis();
		char c = Serial.read();
		if (command != c) {
			switch(c) {
				case 'U': go_Advance(); break;
				case 'L': go_Left();    break;
				case 'R': go_Right();   break;
				case 'D': go_Back();    break;
				case 'C': stop_motor(); break;
				default:break;
			}
			command = c;
		}
	} else {
		if (command != 'C') {
			if ((millis() - elapsed_time) > 1000) {
				stop_motor();
				command = 'C';
			}
		}
	}
}

#define JOYSTICK_LX  34
#define JOYSTICK_LY  33
#define JOYSTICK_LSW 32
#define JOYSTICK_RX  39
#define JOYSTICK_RY  36
#define JOYSTICK_RSW 35

void setup() 
{
	Serial.begin(115200);
}

void loop()
{
	uint16_t Lx, Ly, Rx,Ry;

	Lx = analogRead(JOYSTICK_LX);
	Ly = analogRead(JOYSTICK_LY);
	Rx = analogRead(JOYSTICK_RX);
	Ry = analogRead(JOYSTICK_RY);
	Serial.printf("Lx:%d Ly:%d Rx:%d Ry:%d\n",Lx,Ly,Rx,Ry);
	delay(500);
}

void setup() 
{
	while(1) {
		if (digitalRead(JOYSTICK_LSW) == LOW) break;
		delay(100);
	}

	LXcenter = 0; LYcenter = 0; RXcenter = 0; RYcenter = 0;
	for (int i=0; i<30; i++) {
		LXcenter += analogRead(JOYSTICK_LX);
		LYcenter += analogRead(JOYSTICK_LY);
		RXcenter += analogRead(JOYSTICK_RX);
		RYcenter += analogRead(JOYSTICK_RY);
	}
	LXcenter /= 30;
	LYcenter /= 30;
	RXcenter /= 30;
	RYcenter /= 30;

	if (!WiFi.config(LOCAL_IP, GATEWAY, SUBNETMASK, PRIMARY_DNS)) while(1);

	WiFi.mode(WIFI_STA);
	WiFiMulti.addAP(SECRET_SSID, SECRET_PASS);

	Serial.print("Waiting for WiFi ");
	while (WiFiMulti.run() != WL_CONNECTED) {
		Serial.write('.');
		delay(500);
	}

	Serial.printf("\nWiFi connected\n");
	IPAddress ip = WiFi.localIP();
	Serial.printf("CONTROLLER: %d:%d:%d:%d\n",ip[0],ip[1],ip[2],ip[3]);

	Serial.printf("login ");
	while (!tc.login(esp8266IP)) {
		Serial.write('.');
		delay(500);
	}
	Serial.printf("\n");
}

char stick(int horizontal, int vertical)
{
	char judgeH = 'C', judgeV = 'C';
	int  diffH = 0, diffV = 0;
	if ((horizontal < 0) && (horizontal < (FREE_PLAY * -1))) {
		judgeH = 'L'; diffH = (horizontal - FREE_PLAY) * -1;
	} else if ((horizontal > 0) && (horizontal > FREE_PLAY)) {
		judgeH = 'R'; diffH = (horizontal - FREE_PLAY);
	}
	if ((vertical < 0) && (vertical < (FREE_PLAY * -1))) {
		judgeV = 'D'; diffV = (vertical - FREE_PLAY) * -1;
	} else if ((vertical > 0) && (vertical > FREE_PLAY)) {
		judgeV = 'U'; diffV = (vertical - FREE_PLAY);
	}
	if ((diffH > diffV)&&(judgeH!='C')) {
		return judgeH;
	} else if ((diffH < diffV)&&(judgeV!='C')) {
		return judgeV;
	} else {
		return 'C';
	}
}

void loop()
{
	int Lx, Ly;
	char stickLeft;
	unsigned char ch;
	static int pos = 0;

	Lx = analogRead(JOYSTICK_LX) - LXcenter;
	Ly = analogRead(JOYSTICK_LY) - LYcenter;
	stickLeft = stick(Lx,Ly);
	tc.write(stickLeft);

	while(tc.available()) {
		ch = tc.read();
		switch (ch) {
			case 0xff:
				pos = 2;
				break;
			default:
				if (pos > 0) {
					pos--;
				} else {
					Serial.write(ch);
				}
				break;
		}
	}
	delay(10);
}

if ((millis() - elapsed_time) > 1000) {
	stop_motor();
	command = 'C';
}