Troubleshooting Arduino Code - Bluetooth Controlled Robot + maze solving feature

Hey everyone,

I created a maze solving robot (which works), and i am adding a bluetooth RC car functionality (which works) as well. I have coded and tested these functions independently and they work . The goal is to power on, have it in a static state waiting for a command via bluetooth app from my cell. So it will await for a direction (RC car), or wait for the maze start button (begins the maze solving sequence). Once the maze solving sequence starts it goes through its loops solving the maze. Originally the maze solving sequence had no bluetooth interface and it began immediately on power up. So now with a bluetooth interface the maze solving sequence does not function correctly. On power up the robot waits for BT connection. Once connection is established it waits for commands. If i just use the RC car portion it works as expected. When i use the Maze solving portion (via the app), it does not solve the maze, it just spins in circles to the left. The app being used is called “roboremo.” If someone could help me figure out why it is not working that would be greatly appreciated.

All the code is not attached. It exceed character limit in post. I posted the beginning portion which i believe is where the problem lies.

PLEASE DON’T JUDGE THE POORLY WRITTEN CODE FORMAT, I AM NEW TO THIS.

#include <SoftwareSerial.h>  

int bluetoothTx = 2; // TX-O pin of BT module to Arduino pin2
int bluetoothRx = 3; // RX-I pin of B module to Arduino pin3

SoftwareSerial bluetooth(bluetoothTx, bluetoothRx);

//sensor readings
int leftCenterReading;
int leftNearReading;
int leftFarReading;
int rightCenterReading;
int rightNearReading;
int rightFarReading;

int leftNudge;
int replaystage;
int rightNudge;

#define leapTime 200
#define led 13

//sensor definitions
int leftCenterSensor = 3;
int leftNearSensor = 4;
int leftFarSensor = 5;
int rightCenterSensor = 2;
int rightNearSensor = 1;
int rightFarSensor = 0;

//motor definitions
int leftMotor1 = 7;
int leftMotor2 = 6;
int rightMotor1 = 5;
int rightMotor2 = 8;

char cmd[100];
int cmdIndex;

char path[30] = {};
int pathLength;
int readLength;


void setup() {   // at power on
  
  
  delay(500); // wait for bluetooth module to start
  
  pinMode(leftMotor1, OUTPUT);
  digitalWrite(leftMotor1, LOW);
  
  pinMode(leftMotor2, OUTPUT);
  digitalWrite(leftMotor2, LOW);
  
  pinMode(rightMotor1, OUTPUT);
  digitalWrite(rightMotor1, LOW);
  
  pinMode(rightMotor2, OUTPUT);
  digitalWrite(rightMotor2, LOW);
  
  
  digitalWrite(led, HIGH);
  delay(1000);
  
  // change Bluetooth module baud rate to 9600:
  
  bluetooth.begin(115200); // Bluetooth default baud is 115200
  
  bluetooth.print("$");
  bluetooth.print("$");
  bluetooth.print("$"); // enter cmd mode
  delay(250);  
  bluetooth.println("U,9600,N"); // change baud to 9600
  
  bluetooth.begin(9600);
  
  cmdIndex = 0;

}


void loop() {
  
  
  if(bluetooth.available()) {
 
    char c = (char)bluetooth.read();
    
    cmd[cmdIndex] = c;  
    if(cmdIndex<99) cmdIndex++;
      
    if(c=='\n') {   // each command ends with '\n'
      exeCmd();
      cmdIndex = 0;
    }  
  
  } 
  
}


void exeCmd() {
  
  if(cmd[0]=='f') {  // forward
    digitalWrite(leftMotor1, HIGH);
    digitalWrite(leftMotor2, LOW);
    digitalWrite(rightMotor1, HIGH);
    digitalWrite(rightMotor2, LOW);
  } 
  
  if(cmd[0]=='q') {  // reverse
    digitalWrite(leftMotor1, LOW);
    digitalWrite(leftMotor2, HIGH);
    digitalWrite(rightMotor1, LOW);
    digitalWrite(rightMotor2, HIGH);  
  } 
  
  if(cmd[0]=='h') {  // stop
    digitalWrite(leftMotor1, LOW);
    digitalWrite(leftMotor2, LOW);
    digitalWrite(rightMotor1, LOW);
    digitalWrite(rightMotor2, LOW);
  }
  
  if(cmd[0]=='c') { // left
    digitalWrite(leftMotor1, LOW);
    digitalWrite(leftMotor2, HIGH);
    digitalWrite(rightMotor1, HIGH);
    digitalWrite(rightMotor2, LOW);
  } 
  
  if(cmd[0]=='y') {  // right
    digitalWrite(leftMotor1, HIGH);
    digitalWrite(leftMotor2, LOW);
    digitalWrite(rightMotor1, LOW);
    digitalWrite(rightMotor2, HIGH);
  } 
  
  if(cmd[0]=='m') {  // solve maze
    maze();
  }

}



void maze(){
  
  pinMode(leftCenterSensor, INPUT);
  pinMode(leftNearSensor, INPUT);
  pinMode(leftFarSensor, INPUT);
  pinMode(rightCenterSensor, INPUT);
  pinMode(rightNearSensor, INPUT);
  pinMode(rightFarSensor, INPUT);
  
  readSensors();                                                                                     
 
 if(leftFarReading<200 && rightFarReading<200 && 
   (leftCenterReading>200 || rightCenterReading>200) ){ 
    straight();                                                                                      
  }
  else{                                                                                              
    leftHandWall();                                                                                   
  }


}


void readSensors(){
  
  leftCenterReading  = analogRead(leftCenterSensor);
  leftNearReading    = analogRead(leftNearSensor);
  leftFarReading     = analogRead(leftFarSensor);
  rightCenterReading = analogRead(rightCenterSensor);
  rightNearReading   = analogRead(rightNearSensor);
  rightFarReading    = analogRead(rightFarSensor);  

void leftHandWall(){
  

  if( leftFarReading>200 && rightFarReading>200){
    digitalWrite(leftMotor1, HIGH);
    digitalWrite(leftMotor2, LOW);
    digitalWrite(rightMotor1, HIGH);
    digitalWrite(rightMotor2, LOW);
    delay(leapTime);
    readSensors();
    
    if(leftFarReading>200 || rightFarReading>200){
      done();
    }
    if(leftFarReading<200 && rightFarReading<200){ 
      turnLeft();
    }
    
  }

Kind Regards,
Shaz

Get the sensors off the hardware serial pins, so you can use them for debugging.

Does it really make sense to use pins 2 and 3 for sensors AND for bluetooth communications?

Edit: The sensors are analog sensors. The pinMode() function does NOTHING for the analog pins. It's mucking with the digital pins on the other side of the board, where the bluetooth device is connected.

Get rid of the pinMode() calls in maze(). The mode of pins should be set in setup(), not in other functions, generally.

The biggest problem, though, is that maze() is called once. It reads the sensors once, and sets the motors going, and then returns.

You need to separate reading bluetooth data from using the data read. On every pass through loop(), look for new data, to change what needs to be done. On every pass through loop(), use the last command to decide what should actually be done, and do that.

Hey PaulS

The reason i had those pinmodes located in maze() and not the setup() was because when it was located in setup, my robot would start moving without even waiting for a bluetooth command (with no connection being established) and putting it there seemed to solve the problem as it would wait for its input action. The big issue i had was for the maze solving portion (when i tested that independently) the maze() routine was originally in the loop() so as you alluded to it would keep being repeated and it worked. So now that it is not in the loop the maze solving technique cant keep looping its algorithm. I thought that by calling the routine from the loop(), to the exeCmd(), to the maze(), it would keep looping as long as the input value it was receiving was there. I guess i was mistaken. I don't quite understand your last explanation in regards to the Bluetooth data, if you could elaborate. Also i have an idea (let me know your thoughts):

What if i right an if statement in the loop stating that if in the exeCmd() routine, i receive my value 'm', i then go to the maze() ? The only problem with this is i don't think there would be any valid way to write this code because that would mean i never get sent to the exeCmd() initially. Can something like this be done?

Also thank you for actually replying back to me i know it is a tall task to troubleshoot someone else's code (especially it being this cluttered). Looking forward to your reply.

Kind Regards, Shaz

Hi, If you use REPLY rather than QUICK REPLY, in the lower left hand of the screen you will see and attachment function.

Use it to attach your ino sketch file.

Tom..... :)

The reason i had those pinmodes located in maze() and not the setup() was because when it was located in setup, my robot would start moving without even waiting for a bluetooth command

Nonsense. The pinMode() calls do not make the robot move. Only analogWrite() and digitalWrite() can do that. Of course, you need to make sure that the values written to the pins IN SETUP() are appropriate.