#include <Makeblock.h>
#include <Arduino.h>
#include <String.h>
#include <SoftwareSerial.h>
#include <Wire.h>
MeDCMotor MotorL(M1);
MeDCMotor MotorR(M2);
MeUltrasonicSensor ultrasonic(PORT_3);
MeBluetooth bluetooth(PORT_5);
const int MAXBUFSZ = 128;
char g_lineBuf[MAXBUFSZ];
int g_bufReadIdx = 0;
// movement settings
const boolean g_debug = true;
const int g_stepPeriod = 50;
const int g_maxStepsForward = 3000 / g_stepPeriod;
const int g_maxTurnSteps = 3000 / g_stepPeriod;
const int g_minSpeed = 45;
const int g_maxSpeed = 255;
const int g_speedFactor = 23;
const float g_leftTrim = .97;
const float g_rightTrim = 1.;
// enums
const int LED_PIN = 13;
const int EBRAKE_STOP = 2; // full stop
const int EBRAKE_WAIT = 1; // wait for "Go" signal
const int EBRAKE_GO = 0; // brakes off and GO!
const int MODE_ULTRASONIC = 0;
const int MODE_REMOTE = 1;
const int ULTRASONIC_INIT_SPEED = 200;
// robo state machine
int g_moveSpeed = ULTRASONIC_INIT_SPEED;
int g_numStepsForward = 0;
int g_numTurnSteps = 0;
boolean g_rightFlag;
int g_ebrake = EBRAKE_STOP;
long g_timeLastBlocked = 0;
uint8_t g_mode = MODE_REMOTE;
void setup()
{
Serial.begin(9600);
Serial.println("Software serial on");
bluetooth.begin(9600);
Serial.println("Bluetooth on");
g_lineBuf[0] = '\0';
ResetUltrasonic(EBRAKE_STOP);
pinMode(LED_PIN, OUTPUT); // "obstructed" LED
Serial.println("Bao ready");
}
void loop()
{
char inDat;
boolean hasBlue = false;
hasBlue = bluetooth.available();
if (hasBlue)
{
ResetUltrasonic(EBRAKE_GO);
inDat = bluetooth.read();
Serial.print(inDat);
readIntoLineBuf(inDat);
if (cmdLineReady())
{
String cmd(g_lineBuf);
if (g_debug)
Serial.println("COMMAND " + cmd);
processCommand(cmd);
}
}
else if (g_mode != MODE_REMOTE
&& millis() % g_stepPeriod == 0)
doUltrasonicCar();
}
bool processCommand(String cmd)
{
if (cmd == "!beep")
{
buzzerOn();
delay(100);
buzzerOff();
}
else if (cmd.startsWith("!fwd "))
Forward(getSpeed(cmd, 0));
else if (cmd.startsWith("!bwd "))
Backward(getSpeed(cmd, 0));
else if (cmd.startsWith("!left "))
TurnLeft(getSpeed(cmd, 0));
else if (cmd.startsWith("!right "))
TurnRight(getSpeed(cmd, 0));
else if (cmd.startsWith("!fwdleft"))
ForwardAndLeft(getSpeed(cmd, 0), getSpeed(cmd, 1));
else if (cmd.startsWith("!fwdright"))
ForwardAndRight(getSpeed(cmd, 0), getSpeed(cmd, 1));
else if (cmd.startsWith("!bwdleft"))
BackwardAndTurnLeft(getSpeed(cmd, 0), getSpeed(cmd, 1));
else if (cmd.startsWith("!bwdright"))
BackwardAndTurnRight(getSpeed(cmd, 0), getSpeed(cmd, 1));
else
Stop();
}
int getSpeed(String cmd, int idx)
{
int mag = getCmdInt(cmd, idx);
return g_speedFactor * mag + g_minSpeed;
}
int getCmdInt(String cmd, int idx)
{
int spaceIdx = cmd.indexOf(' ');
for (int i=0; i < idx; i++)
spaceIdx = cmd.indexOf(' ', spaceIdx+1);
return cmd.substring(spaceIdx+1).toInt();
}
bool cmdLineReady()
{
if (g_bufReadIdx != 0)
return false; // still building line
if (g_lineBuf[0] != '!')
return false; // not a command line
return true;
}
void readIntoLineBuf(char c)
{
if (c == '\n' || c == '\r' || c == '\0')
{
// finish line for processing, set ready for next line
g_bufReadIdx = 0;
return;
}
if (g_bufReadIdx >= MAXBUFSZ - 1)
{
// drop overflow bytes
return;
}
g_lineBuf[g_bufReadIdx] = c;
g_lineBuf[g_bufReadIdx+1] = '\0';
g_bufReadIdx++;
}
void doUltrasonicCar()
{
int distance = ultrasonic.distanceCm();
boolean isBlocked = (distance > 0 && distance < 60);
if (isBlocked)
g_timeLastBlocked = millis();
else if (millis() - g_timeLastBlocked < 100)
isBlocked = true;
if (isBlocked)
digitalWrite(LED_PIN, HIGH);
else
digitalWrite(LED_PIN, LOW);
boolean isStuck
= (g_numTurnSteps > g_maxTurnSteps
|| g_numStepsForward > g_maxStepsForward);
if (g_ebrake == EBRAKE_STOP && !isBlocked)
{
// /unstuck, ready for go signal
g_ebrake = EBRAKE_WAIT;
return;
}
else if (g_ebrake == EBRAKE_WAIT && isBlocked)
{
// "Go!"
ResetUltrasonic(EBRAKE_GO);
return;
}
else if (g_ebrake == EBRAKE_GO && isStuck)
{
// stuck -- yank ebrake
g_ebrake = EBRAKE_STOP;
Stop();
return;
}
if (g_ebrake != EBRAKE_GO)
{
// halt until !blocked and receive "Go" signal
return;
}
if (isBlocked)
{
// evasive maneuvers!
randomSeed(analogRead(A4));
int randnum = random(300);
if (distance > 30)
{
if (randnum > 150 && !g_rightFlag)
TurnLeft(g_moveSpeed);
else
TurnRight(g_moveSpeed);
}
else
{
if (randnum > 150)
BackwardAndTurnLeft(g_moveSpeed, g_moveSpeed);
else
BackwardAndTurnRight(g_moveSpeed, g_moveSpeed);
}
}
else
{
// ONWARD!
Forward(g_moveSpeed);
}
}
void ResetUltrasonic(int brakePos)
{
g_rightFlag = false;
g_numStepsForward = 0;
g_numTurnSteps = 0;
g_ebrake = brakePos;
if (g_debug && g_mode == MODE_ULTRASONIC)
Serial.println("Reset");
}
void Stop()
{
MotorL.stop();
MotorR.stop();
}
void Forward(int speed)
{
g_numStepsForward++;
g_numTurnSteps = 0;
g_rightFlag = false;
MotorL.run(speed * g_leftTrim);
MotorR.run(speed * g_rightTrim);
if (g_debug)
Serial.println("Forward");
}
void ForwardAndLeft(int fspeed, int lspeed)
{
g_numStepsForward++;
g_numTurnSteps++;
g_rightFlag = false;
MotorL.run(fspeed - (lspeed / 2));
MotorR.run(fspeed);
if (g_debug)
Serial.println("Forward+Left");
}
void ForwardAndRight(int fspeed, int rspeed)
{
g_numStepsForward++;
g_numTurnSteps++;
g_rightFlag = true;
MotorL.run(fspeed);
MotorR.run(fspeed - (rspeed / 2));
if (g_debug)
Serial.println("Forward+Right");
}
void Backward(int speed)
{
g_rightFlag = false;
g_numTurnSteps = 0;
MotorL.run(-speed);
MotorR.run(-speed);
if (g_debug)
Serial.println("Backward");
}
void TurnLeft(int speed)
{
g_rightFlag = false;
g_numStepsForward = 0;
g_numTurnSteps++;
MotorL.run(-speed / 2);
MotorR.run(speed / 2);
if (g_debug)
Serial.println("Left");
}
void TurnRight(int speed)
{
g_rightFlag = true;
g_numStepsForward = 0;
g_numTurnSteps++;
MotorL.run(speed / 2);
MotorR.run(-speed / 2);
if (g_debug)
Serial.println("Right");
}
void BackwardAndTurnLeft(int bspeed, int lspeed)
{
g_rightFlag = true;
g_numStepsForward = 0;
g_numTurnSteps++;
MotorL.run(-bspeed + (lspeed / 2));
MotorR.run(-bspeed);
if (g_debug)
Serial.println("Backward+Left");
}
void BackwardAndTurnRight(int bspeed, int rspeed)
{
g_rightFlag = false;
g_numStepsForward = 0;
g_numTurnSteps++;
MotorL.run(-bspeed);
MotorR.run(-bspeed + (rspeed / 2));
if (g_debug)
Serial.println("Backward+Right");
}
void ChangeSpeed(int spd)
{
g_moveSpeed = spd;
}
