/*************************************************
Copyright:wiki.labplus.cn
Author:tangliufeng
Date:10/8/2018
Description:由arduino电机驱动板加UNO+循迹模块+红外模块拼装的小车
1.循迹功能：按键“*”，进入循迹模式
2.红外遥控：按键“#”，进入遥控模式，使用方向键控制小车行走
2.避障：按键“0”，进入避障模式
**************************************************/
#include <Wire.h>
#include <SoftwareSerial.h>
#include <arduino.h>

char info[50];
uint8_t Car_Mode = 0;
uint8_t Move = 0;
uint8_t buf;

#define LEFT_SPEED 80
#define RIGHT_SPEED 100


//IR Button Value
#define IR_1 0x00
#define IR_2 0x80
#define IR_3 0x40
#define IR_4 0x20
#define IR_5 0xA0
#define IR_6 0x60
#define IR_7 0x10
#define IR_8 0x90
#define IR_9 0x50
#define IR_0 0xB0
#define IR_star 0x30
#define IR_well 0x70
#define IR_UP 0x88
#define IR_DWN 0x98
#define IR_L 0x28
#define IR_R 0x68
#define IR_OK 0xA8


SoftwareSerial irSerial(2, 3);  //rx P2,tx P3  connection IR Receive moudle
void setup()
{
	Wire.begin();
	Serial.begin(115200);
	irSerial.begin(115200);
	pinMode(5, OUTPUT);
	pinMode(6, OUTPUT);
	pinMode(9, OUTPUT);
	pinMode(10, OUTPUT);

	pinMode(A1, INPUT);
	pinMode(A2, INPUT);
	forward(LEFT_SPEED, RIGHT_SPEED);

}
void loop()
{
	if (irSerial.available() > 0)
	{

		buf = irSerial.read();
		//Serial.println(buf, HEX);

		switch (buf)
		{
		case IR_star:
			Car_Mode = 0;
			Serial.println("tracking mode");

			break;
		case IR_well:
			Car_Mode = 1;
			Serial.println("control mode");
			break;
		case IR_0:
			Car_Mode = 2;
			Serial.println("elude mode");
			break;
		case IR_UP:
			Move = 1;
			break;
		case IR_DWN:
			Move = 2;
			break;
		case IR_L:
			Move = 3;
			break;
		case IR_R:
			Move = 4;
			break;
		case IR_OK:
			Move = 0;
			break;
		default:
			break;
		}

		buf = 0;

	}

	if (Car_Mode == 1)
	{
		switch (Move)
		{
		case 0:
			stop();
			break;
		case 1:
			forward(LEFT_SPEED, RIGHT_SPEED);
			break;
		case 2:
			back(LEFT_SPEED, RIGHT_SPEED);
			break;
		case 3:
			turnLeft(RIGHT_SPEED);
			break;
		case 4:
			turnRight(LEFT_SPEED);
			break;
		default:
			break;
		}
	}

	if (Car_Mode == 2)
	{
		//Serial.println(Distance());
		if (Distance() < 12)
		{
			stop();
			back(LEFT_SPEED, RIGHT_SPEED);
			delay(500);
			analogWrite(5, RIGHT_SPEED);
			analogWrite(6, 0);
			analogWrite(9, LEFT_SPEED);
			analogWrite(10, 0);
			delay(200);

		}
		else
		{
			forward(LEFT_SPEED, RIGHT_SPEED);
		}
		delay(10);

	}
	if (Car_Mode == 0)
	{
		tracking();
	}



}


void tracking()
{
	//sprintf(info, "D1:%d D2:%d\r\n", digitalRead(A1), digitalRead(A2));
	//Serial.print(info);
	if (digitalRead(A1) == 0 && digitalRead(A2) == 1)
	{
		turnLeft(RIGHT_SPEED);
		Serial.println("Left");
	}
	else if (digitalRead(A1) == 1 && digitalRead(A2) == 0)
	{
		turnRight(LEFT_SPEED);
		Serial.println("Right");
	}
	else
	{
		forward(LEFT_SPEED, RIGHT_SPEED);
	}
}

void forward(uint8_t left_speed, uint8_t right_speed)
{
	analogWrite(5, right_speed);
	analogWrite(6, 0);
	analogWrite(9, 0);
	analogWrite(10, left_speed);

}

void back(uint8_t left_speed, uint8_t right_speed)
{
	analogWrite(5, 0);
	analogWrite(6, right_speed);
	analogWrite(9, left_speed);
	analogWrite(10, 0);

}

void stop()
{
	analogWrite(5, 0);
	analogWrite(6, 0);
	analogWrite(9, 0);
	analogWrite(10, 0);

}

void turnLeft(uint8_t speed)
{
	analogWrite(5, speed);
	analogWrite(6, 0);
	analogWrite(9, 0);
	analogWrite(10, 0);

}

void turnRight(uint8_t speed)
{

	analogWrite(5, 0);
	analogWrite(6, 0);
	analogWrite(9, 0);
	analogWrite(10, speed);


}


uint8_t Distance()
{
	int i = 0;
	uint8_t cm;
	uint8_t temp[2];
	Wire.beginTransmission(0x0b);              //传输给从机设备0x0B
	Wire.write(1);                             //发送1指令
	Wire.endTransmission();                    //结束传输
	Wire.requestFrom(0x0b, 2);                 //接收从机设备0x0B，长度2字节
	while (Wire.available()) {
		temp[i] = Wire.read();
		i++;
	}
	return  cm = (temp[0] + temp[1] * 256) / 10;
}