I have built a differential steering rover. Drive motors are HS-311 continuous rotation servos, I have 3 HC-SR04 ultrasonic sensors and I am using HC06 bluetooth module from vertronix. Battery pack is 6x AA (9volts) and the arduino for now is powered through USB. I am also using standard libraries downloaded from arduino.
I have tested the motors and ultrasonics separately and they are working fine. I can run the robot with an autonomous avoider sketch and it works. With my bluetooth remote piece, I can't even get it to go autonomous...motors don't run with the new sketch.
I've tried debugging by using led 13 on/off signals put at various points and sometimes it seems it makes one full loop and other times it gets caught just after initializing bluetooth. I have attached my current sketch as a text file.
Thanks for any help.
#include <LUltrasonic.h>
#include <SoftwareSerial.h>
#include <Servo.h>
void measureDistances();
void findPathLoop();
void moveLoop();
void remoteControlLoop();
//bluetooth data
char data;
char incomingByte;
#define RxD 12
#define TxD 13
SoftwareSerial blueToothSerial(RxD,TxD);
long duration, distance, leftsensor, rightsensor, centersensor;
unsigned long lastTimeCommand, autoOFF = 1000;
//ULTRASONIC
const int trigPinFrontLeft = 9;
const int pingEchoFrontLeft = 8;
const int trigPinCenter = 11;
const int pingEchoCenter = 10;
const int trigPinFrontRight = A3;
const int pingEchoFrontRight = A2;
int maximumRange = 300;
int minimumRange = 0;
//long duration, distance;
//MOTOR CONTROLLER
Servo leftmotor;
Servo rightmotor;
//STATES
typedef enum tagStateType
{
StateTypeUnknown,
StateTypeMove,
StateTypeFindPath,
StateTypeRemoteControl
}
StateType;
StateType currentStateType = StateTypeMove;
StateType previousStateType;
//SETUP
void setupBlueToothConnection()
{
blueToothSerial.begin(9600);
blueToothSerial.print("\r\n+STWMOD=0\r\n");
blueToothSerial.print("\r\n+STNA=SeeedBTSlave\r\n");
blueToothSerial.print("\r\n+STOAUT=1\r\n");
blueToothSerial.print("\r\n+STAUTO=0\r\n");
delay(2000);
blueToothSerial.print("\r\n+INQ=1\r\n");
delay(2000);
blueToothSerial.flush();
}
void setup()
{
// Serial.begin(9600);
leftmotor.attach(4); //D4
rightmotor.attach(5); //D5
leftmotor.writeMicroseconds(1499);
rightmotor.writeMicroseconds(1560); // Stop
pinMode(trigPinFrontLeft, OUTPUT);
pinMode(pingEchoFrontLeft, INPUT);
pinMode(trigPinFrontRight, OUTPUT);
pinMode(pingEchoFrontRight, INPUT);
pinMode(trigPinCenter, OUTPUT);
pinMode(pingEchoCenter, INPUT);
pinMode(RxD, INPUT);
pinMode(TxD, OUTPUT);
setupBlueToothConnection();
long duration, leftsensor, rightsensor, centersensor;
}
//LOOPS
void loop()
{
if (currentStateType != StateTypeRemoteControl)
{
if (blueToothSerial.available())
currentStateType = StateTypeRemoteControl;
else
measureDistances();
}
switch (currentStateType)
{
case StateTypeMove:
moveLoop();
break;
case StateTypeFindPath:
findPathLoop();
break;
case StateTypeRemoteControl:
remoteControlLoop();
break;
}
}
void moveLoop()
{
if (previousStateType != currentStateType)
{
leftmotor.writeMicroseconds(899);
rightmotor.writeMicroseconds(2160);
previousStateType = currentStateType;
}
if (centersensor >=25)
{
leftmotor.writeMicroseconds(899);
rightmotor.writeMicroseconds(2160);
}
if (centersensor <25)
{
leftmotor.writeMicroseconds(1499);
rightmotor.writeMicroseconds(1560);
currentStateType = StateTypeFindPath;
}
}
void findPathLoop()
{
if (previousStateType != currentStateType)
{
leftmotor.writeMicroseconds(899);
rightmotor.writeMicroseconds(1140);
previousStateType = currentStateType;
}
if (leftsensor > rightsensor) //turn left if left is clear
{
leftmotor.writeMicroseconds(1020);
rightmotor.writeMicroseconds(2160);
delay (3000);
}
if (leftsensor < rightsensor) //turn right if right is clear
{
leftmotor.writeMicroseconds(899);
rightmotor.writeMicroseconds(1140);
delay (3000);
}
{
// motorController.stopMoving();
leftmotor.writeMicroseconds(1499);
rightmotor.writeMicroseconds(1560); // Stop
currentStateType = StateTypeMove;
}
}
void remoteControlLoop()
{
if (blueToothSerial.available())
{
incomingByte = blueToothSerial.read();
char ch = Serial1.read();
if (ch == 'f') {
//go forward
leftmotor.writeMicroseconds(899);
rightmotor.writeMicroseconds(2160);
}
if (ch == 'b') {
// go backwards
leftmotor.writeMicroseconds(2099);
rightmotor.writeMicroseconds(960);
}
if (ch == 'l') {
// turn left
leftmotor.writeMicroseconds(1020);
rightmotor.writeMicroseconds(2160);
}
if (ch == 'r') {
// turn right
leftmotor.writeMicroseconds(899);
rightmotor.writeMicroseconds(1140);
}
if (ch == 's') {
// stop
leftmotor.writeMicroseconds(1499);
rightmotor.writeMicroseconds(1560);
}
delay(10);
}
lastTimeCommand = millis();
if (millis() >= (lastTimeCommand + autoOFF))
{
// motorController.stopMoving();
leftmotor.writeMicroseconds(1499);
rightmotor.writeMicroseconds(1560); // Stop
currentStateType = StateTypeMove;
previousStateType = StateTypeRemoteControl;
}
}
//MEASURE DISTANCES
void measureDistances()
{
long duration, leftsensor, rightsensor, centersensor;
leftsensor=40; //default distances in case ultrasonic sensor stops working. This
//will make the robot go in circles.
rightsensor=40;
centersensor=50;
//{
//Before moving we need to check the sensors
digitalWrite(trigPinFrontLeft, LOW);
delayMicroseconds(2);
digitalWrite(trigPinFrontLeft, HIGH);
delayMicroseconds(10);
digitalWrite(trigPinFrontLeft, LOW);
duration = pulseIn(pingEchoFrontLeft, HIGH);
// convert the time into a distance
leftsensor = microsecondsToCentimeters(duration);
// calculate the distance in cm based on speed of sound
//distance = duration/58.2; enable to use com display to read ultrasonic sensor
//Serial.println(distance); enable to use com display to read ultrasonic sensor
digitalWrite(trigPinFrontRight, LOW);
delayMicroseconds(2);
digitalWrite(trigPinFrontRight, HIGH);
delayMicroseconds(5);
digitalWrite(trigPinFrontRight, LOW);
duration = pulseIn(pingEchoFrontRight, HIGH);
rightsensor = microsecondsToCentimeters(duration);
digitalWrite(trigPinCenter, LOW);
delayMicroseconds(2);
digitalWrite(trigPinCenter, HIGH);
delayMicroseconds(5);
digitalWrite(trigPinCenter, LOW);
duration = pulseIn(pingEchoCenter, HIGH);
centersensor = microsecondsToCentimeters(duration);
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29/ 2;
}
rover.txt (6.36 KB)