#include

#include

SoftwareSerial mySerial(4,5);

#define BLESerial mySerial

#include

String var_variable;

int var_black;

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;

}

}

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 "";

}

Motor MotorLeft(MOTOR0_PINA, MOTOR0_PINB);

Motor MotorRight(MOTOR1_PINA, MOTOR1_PINB);

void setup()

{

BLESerial.begin(9600);

MotorLeft.begin();

MotorRight.begin();

}

void loop()

{

String BLE_Receive = readserail();

if (BLE_Receive != "")

{

var_variable=solution(BLE_Receive, "show_S1");

}

delay(10);

if(((var_variable) == "begin"))

{

while(!(!(((var_variable) == "begin"))))

{

String BLE_Receive = readserail();

if (BLE_Receive != "")

{

var_variable=solution(BLE_Receive, "show_S1");

}

delay(10);

var_black=analogRead(A6);

}

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

{

MotorLeft.setSpeed(40);

MotorRight.setSpeed(-40);

}

else

{

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

{

MotorLeft.setSpeed(40);

MotorRight.setSpeed(0);

}

else

{

MotorLeft.setSpeed(0);

MotorRight.setSpeed(-40);

}

}

}

if(((var_variable) == "stop"))

{

MotorLeft.Brake();

MotorRight.Brake();

}

if(((var_variable) == "up"))

{

MotorLeft.setSpeed(60);

MotorRight.setSpeed(-60);

}

if(((var_variable) == "left"))

{

MotorRight.setSpeed(0);

MotorLeft.setSpeed(40);

}

if(((var_variable) == "right"))

{

MotorRight.setSpeed(-40);

MotorLeft.setSpeed(0);

}

}