#include

#include

#include

#include

SoftwareSerial mySerial(4,5);

#define BLESerial mySerial

int var_speed;

int var_black;

U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE);

String solution(String _sta, char *c)

{

String data;

if (strstr(_sta.c_str(), c) == NULL)

return "";

else

{

if (_sta.startsWith("{") && _sta.endsWith("}"))

{

_sta = _sta.substring(1, _sta.length() - 1);

_sta.replace("\"", "");

uint8_t _length = _sta.length();

char buf[_length];

char c_all[30] = "";

char data1[] = ":%s";

strcat(c_all, c);

strcat(c_all, data1);

sscanf(_sta.c_str(), c_all, &buf);

data = String(buf);

}

if (data != NULL)

return data;

}

}

Motor MotorLeft(MOTOR0_PINA, MOTOR0_PINB);

Motor MotorRight(MOTOR1_PINA, MOTOR1_PINB);

void BLESenddata(String _st, String _data)

{

String send_data;

int8_t number;

send_data = "{\"";

send_data += _st;

send_data += "\":\"";

send_data += _data;

send_data += "\"}";

number = send_data.length() / 17;

if (number == 0)

{

BLESerial.println(send_data);

delay(30);

}

else

{

while (number >= 0)

{

BLESerial.print(send_data.substring(0, 17));

send_data = send_data.substring(17, send_data.length());

delay(30);

number--;

}

BLESerial.print("\n");

}

}

String uartMsg;

uint8_t uartStep = 0;

String readserail()

{

char inByte = BLESerial.read();

switch (uartStep)

{

case 0:

uartMsg = "";

if (inByte == '{')

{

uartMsg += inByte;

uartStep = 1;

}

break;

case 1:

uartMsg += inByte;

if (inByte == '}')

{

uartStep = 0;

return uartMsg;

}

break;

default:

break;

}

return "";

}

void setup()

{

Serial.begin(9600);

MotorLeft.begin();

MotorRight.begin();

BLESerial.begin(9600);

var_speed=40;

var_black=analogRead(A6);

}

void loop()

{

u8g.setFont(u8g_font_9x15);

u8g.firstPage();

do

{

u8g.setPrintPos(10, 10);

u8g.print((var_black));

u8g.setPrintPos(10, 30);

u8g.print((analogRead(A0)));

u8g.setPrintPos(10, 50);

u8g.print((analogRead(A2)));

}

while ( u8g.nextPage() );

String BLE_Receive = readserail();

if (BLE_Receive != "")

{

delay(0.1*1000);

Serial.println(solution(BLE_Receive, "show_S1"));

if(((solution(BLE_Receive, "show_S1")) == "begin"))

{

var_speed=40;

MotorLeft.setSpeed(var_speed);

MotorRight.setSpeed(0-(var_speed));

BLESenddata("control_C1", String("1"));

delay(30);

}

if(((solution(BLE_Receive, "show_S1")) == "stop"))

{

var_speed=0;

MotorLeft.setSpeed(0);

MotorRight.setSpeed(0);

BLESenddata("control_C1", String("2"));

delay(30);

}

if(((solution(BLE_Receive, "show_S1")) == "up"))

{

var_speed=70;

MotorLeft.setSpeed(var_speed);

MotorRight.setSpeed(0-(var_speed));

BLESenddata("control_C1", String("3"));

delay(30);

}

if(((solution(BLE_Receive, "show_S1")) == "left"))

{

for(int i=0;i<10;i++)

{

var_speed=40;

MotorLeft.setSpeed((var_speed)-0);

MotorRight.setSpeed(var_speed);

BLESenddata("control_C1", String("4"));

delay(30);

}

}

if(((solution(BLE_Receive, "show_S1")) == "right"))

{

for(int i=0;i<10;i++)

{

var_speed=40;

MotorLeft.setSpeed(0-(var_speed));

MotorRight.setSpeed(0-(var_speed));

BLESenddata("control_C1", String("5"));

delay(30);

}

}

}

delay(10);

if(((((analogRead(A0)) < (var_black))) && (((analogRead(A2)) < (var_black)))))

{

MotorLeft.setSpeed(var_speed);

MotorRight.setSpeed(0-(var_speed));

}

if(((((analogRead(A0)) (var_black)))))

{

MotorLeft.setSpeed(var_speed);

MotorRight.setSpeed(0);

}

if(((((analogRead(A2)) (var_black)))))

{

MotorLeft.setSpeed(0);

MotorRight.setSpeed(0-(var_speed));

}

}