void loop()
{
checkBTcmd(); // verify if a comand is received from BT remote control
receiveCmd ();
if (turnOn) manualCmd ();
else stopRobot ();
}
void checkBTcmd()
{
if (BT1.available())
{
command = BT1.read();
BT1.flush();
}
}
void receiveCmd ()
{
switch (command)
{
case 'p':
Serial.println("command ==> p");
turnOn = !turnOn;
command = 0;
analogWrite(ledStatus, turnOn*128); // Robot ON - Led ON
beep(outBuz, 1000, 100);
BT1.print(" COMMAND ON/OFF");
BT1.println('\n');
delay(200); //Delay to call attention to mode change
break;
case 'm': //not used here
break;
}
}
void manualCmd()
{
switch (command)
{
case 'f':
moveStop(); //turn off both motors
state = command;
break;
case 'w':
moveForward();
state = command;
break;
case 'd':
moveRight();
break;
case 'a':
moveLeft();
break;
case 's':
moveBackward();
state = command;
break;
case '+':
if (state == 'w')
{
motorSpeed = motorSpeed + 10;
if (motorSpeed > MAX_SPEED)
{
motorSpeed = MAX_SPEED;
}
command = 'w';
} else {command = state;}
break;
case '-':
if (state == 'w')
{
motorSpeed = motorSpeed - 10;
}
if (motorSpeed < MIN_SPEED )
{
motorSpeed = MIN_SPEED;
}
Serial.println(motorSpeed);
command = state;
break;
}
}
void moveForward() // rear motor FW
{
analogWrite(rearMtEne, motorSpeed);
digitalWrite(rearMtFw, HIGH);
digitalWrite(rearMtBw, LOW);
digitalWrite(frontMtEne, LOW);
digitalWrite(ledRed, LOW);
delay(5);
}
void stopRobot ()
{
digitalWrite(ledBlue, LOW);
digitalWrite(ledRed, LOW);
state = 0;
moveStop(); //turn off both motors
}
// Motor Drives
const int rearMtFw = 4; // Rear Motor - FW
const int rearMtBw = 7; // Rear Motor - BW
const int rearMtEne = 6; // Rear Motor - enable
const int frontMtLeft = 2; // Front Motor - turn Left
const int frontMtRight = 3; // Front Motor - turn right
const int frontMtEne = 5; // Front Motor enable
void moveForward() // rear motor FW
{
analogWrite(rearMtEne, motorSpeed);
digitalWrite(rearMtFw, HIGH);
digitalWrite(rearMtBw, LOW);
digitalWrite(frontMtEne, LOW);
digitalWrite(ledRed, LOW);
delay(5);
}
void moveBackward() // rear motor BW
{
analogWrite(rearMtEne, motorSpeed);
digitalWrite(rearMtFw, LOW);
digitalWrite(rearMtBw, HIGH);
digitalWrite(frontMtEne, LOW);
digitalWrite(ledRed, HIGH);
delay(5);
}
void moveLeft() // front motor left
{
digitalWrite(frontMtEne, HIGH);
digitalWrite(frontMtLeft, HIGH);
digitalWrite(frontMtRight, LOW);
digitalWrite(ledRed, LOW);
delay(10);
}
//******************************************************************************//
void moveRight() // front motor right
{
digitalWrite(frontMtEne, HIGH);
digitalWrite(frontMtLeft, LOW);
digitalWrite(frontMtRight, HIGH);
digitalWrite(ledRed, LOW);
delay(10);
}
void stopRobot ()
{
digitalWrite(ledBlue, LOW);
digitalWrite(ledRed, LOW);
state = 0;
moveStop(); //turn off both motors
}