ArcTuRus v1.1 build modifications
Continuing on from ArcTuRus v1.0, I had a few thoughts about improvements.
What I had not anticipated was the challenge of time. Or, the lack of time to spend on working on the robot. The original version was worked on towards the end of September 2015 and it was now coming up to mid-November, where had the time gone?!
I had put ArcTuRus back on the shelf, but I did have a plan in the back of my mind of how I was going to refactor a few things. For instance, I am using 2 RoMeo Arduino boards. The first was pretty much doing everything and the second was just doing speech.
The logic was that RoMeo#1 would do the sensing via the PING and IR sensor (and possibly the PIXY) and send a Bluetooth message to RoMeo#2 that would then use the Speech module to shout out loud "moving left" (or words to that effect), but RoMeo#1 would also be operating the motor servos to steer the robot away from hitting anything.
Whilst this was fine and was working okay. I still felt it was a bit unbalanced. After-all, if RoMeo#2 was being informed to "move left", "move right", "go ahead", "stop", etc... then why not connect up the motor servos to that piece of code.
Made perfect sense and seemed like a simple enough job. Which is why the thought stayed in the back of my brain not really going anywhere. It was only a matter of taking the same code from RoMeo#1 and pasting it into RoMeo#2, re-uploading and job done.
Well, in theory that was correct. In reality, not quite that simple. Hence, doing this write-up at 4am! (Well, 5am now, but you get the idea)
From a high-level, the above was what was needed. Seemed simple enough.
Initially, I ran into problems passing too much data from RoMeo#1 to RoMeo#2 via the BLE connection. The original code on RoMeo#2 was listening for any data being sent and when data was detected it would continue reading until the end of the String, before moving on and processing the String of data it was sent.
This is where flaw#1 came in. For some odd reason RoMeo#1 IR sensor was going a bit crazy, it was sending "move left" signals ever second... which as you can imagine gets a bit tiresome when all you hear from the speak is the robot noise of "move left, move left, move left....".
Looking closer at the debug output from RoMeo#2, I could see that the String contained a long string like this, MOVE,LEFT,MOVE,LEFT,MOVE,LEFT, etc...
Now, the code on RoMeo#1 was indeed doing BlueToothSerial.print() commands when the robot was meant to be moving forward (MOVE) and if it detected something with the IR sensor then it was meant to move left (LEFT) with a different BlueToothSerial.print() command. It seemed that because the transmission of the commands was within the same 10milliseconds RoMeo#2 was combining all of the messages together.
After a bit of a re-code, I changed the RoMeo#2 BLE to listen a character at a time, rather than a whole String. Sorted. Well, that's what I initially thought.
I ended up having a re-think about what was being sent from RoMeo#1 to RoMeo#2 and I thought, "why send such long text, when all I really need to send is a couple of characters to distinguish each action?". So, that is what I did.
I would just read a couple of chars that were passed over and execute the correct action, ie. send the signals to the servos to move the wheels to steer left. But, I was noticing in the debug output that the chars being received/read was sometimes getting out of sync and the code couldn't make sense of what was being received.
I was still getting the freaky 'move left' loop, which was causing the overflow of data. I added an extra line of code so that if I couldn't work out what was sent, then I must have received a dodgy character, so I needed to do an extra read() to get the code re-aligned. LovelyHack#1 It did the job and seems to be doing okay, so we'll leave that there, until it comes back to bite us when it goes into Production..... :-D
I then thought, it's probably about time to figure out why the IR Sensor is freaking out all the time and telling me to "move left". Debug mode turned on.... and I see for some odd reason the distance value that the code thinks it's detecting is about 11cm. Which is below the 20cm "object detected" setting, so it was doing the right thing, albeit there was no object at 11cm.
I repositioned the robot, having over 1m of clear space infront of it, still getting the odd readings and the "move left" speech.
More debugging.... more head scratching. Then I noticed that the WiFi card was not always connecting to the WiFi (blue status light would still be flashing), which is odd, that was always connecting okay before.
So, I did the car mechanic approach. Strip it all back to the basics and build it back up again piece by piece until you find the last piece you put on breaks it.
And there we have it...it turned out, (bizarrely!) that the PIXY camera was causing the problem.
In the code for RoMeo#1, I had a boolean flag that I was using to choose to ignore the PIXY, that was set to false, so the PIXY code wasn't getting executed. I switched it to true and, "huh?", the IR sensor was now perfectly fine and outputting the correct distance detection values.
What on earth was going on here? I fired up 'PixyMon' and plugged in the older USB cable directly to the PIXY, yep, I could see live streaming of the camera on my Mac, so the PIXY is still working.....
...oh, and then I notice it....all the settings were gone. All of my previously trained colour detections had gone.... the PIXY had "lost its memory". Probably due to the 5-6 weeks of non-usage/power supply? I had just assumed that it would keep the settings in a chip in memory or something and when power was restored it would just re-use the values, y'know, like if you download code to the Arduino and don't power the Arduino for a year, it'll still have the program inside it and it'll execute it. It won't just forget it.
I then cursed myself for not making a note of the previous PIXY setting values, as I'll have to retrain it again. But it did seem like now everything was working as it should! Phew!
In my euphoria, I also decided to repurpose some of the buttons on RoMeo#1. I had a start() and stop() button assigned, so I thought I would setup the other buttons to alter the boolean flags for bluetooth(), debugMode() and pixyEnabled().
The bluetooth() one is actually very handy as it turns off the BlueToothSerial.print() commands, which stops the speech and motor servos from being activated. Handy feature to have, from 1-4am :-D
Arduino Code
RoMeo#1
#include <NewPing.h>
#include <elapsedMillis.h>
#include <SPI.h>
#include <Pixy.h>
#include <SoftwareSerial.h>
#include <SparkFunESP8266WiFi.h>
//advance X1 speed
//:loop
// detect PING/IR Sensor values
// if PING <= 20cm stop, turnR A1
// if IR <= 20cm stop, turnL A2
// advance X2 speed
//
const char mySSID[] = "virginmedia1379311";
const char myPSK[] = "password";
const char destServer[] = "example.com";
const String httpRequest = "GET / HTTP/1.1\nHost: example.com\nConnection: close\n\n";
boolean debugMode = true; //if true outputs to Serial Monitor
boolean pixyEnabled = false; //allows ability to not use PIXY
boolean bleEnabled = true; //if true communicates over Bluetooth
boolean keepMovingForward = false;
//Standard PWM DC control PINS //For Romeo v1.1
//int E1 = 5; //M1 Speed Control
//int E2 = 6; //M2 Speed Control
//int M1 = 4; //M1 Direction Control
//int M2 = 7; //M1 Direction Control
int adc_key_val[5] ={30, 150, 360, 535, 760 };
int NUM_KEYS = 5;
int adc_key_in;
int key=-1;
int oldkey=-1;
#define RxD 3
#define TxD 2
SoftwareSerial BlueToothSerial(RxD,TxD);
#define STARTpin A2 //StartButton Pin
#define SHARPpin A1 //IRPin Analog
#define PINGInterval 1250 //milliseconds to wait for checking PING and IR Sensors
const int triggerPingPin1 = 11; //PingPin
const int maxDistance = 200; //cm
const int triggerDetectDistance = 15; //20; //cm
const int triggerStopDistance = 5; //cm
elapsedMillis PINGTIMER;
NewPing sonar1(triggerPingPin1, triggerPingPin1, maxDistance);
Pixy pixy; //doesn't mention it anywhere but I think this takes PIN 10
// Convert ADC value to key number
int get_key(unsigned int input)
{
int k;
for (k = 0; k < NUM_KEYS; k++)
{
if (input < adc_key_val[k])
{
return k;
}
}
if (k >= NUM_KEYS)
k = -1; // No valid key pressed
return k;
}
void setup(void)
{
Serial.begin(9600);
if(bleEnabled) {
BlueToothSerial.begin(38400);
}
delay(100);
callBLE("M1");
delay(1000);
if(debugMode) { Serial.println("Starting Robot"); }
//have to init the WiFi shield BEFORE anything else else it doesn't work
initializeESP8266(); //init shield
connectESP8266(); //connect to WiFi network
if(debugMode) {
displayConnectInfo(); //show the IP Address assigned
// clientDemo();
}
//init the IR Analog PIN
pinMode(SHARPpin, INPUT);
//init the PINS for the wheels
for(int i=4;i<=7;i++) {
pinMode(i, OUTPUT);
}
//init the PIXY camera
if(pixyEnabled) { pixy.init(); }
callBLE("M2");
delay(1000);
Serial.println("Started robot");
//we'll use the debug LED to output a heartbeat - just to know we're still alive
pinMode(13, OUTPUT);
}
void loop(void)
{
adc_key_in = analogRead(7); // read the value from the sensor
digitalWrite(13, HIGH);
/* get the key */
key = get_key(adc_key_in); // convert into key press
if (key != oldkey) { // if keypress is detected
delay(50); // wait for debounce time
adc_key_in = analogRead(7); // read the value from the sensor
key = get_key(adc_key_in); // convert into key press
if (key != oldkey) {
oldkey = key;
if (key >=0){
// Serial.println(adc_key_in, DEC);
Serial.print("key=");
Serial.println(key);
if(key == 1) { //up
//start the robot moving
callBLE("KE");
keepMovingForward = true;
// advance(100,100);
}
if(key == 2) { //down
//manually stop the robot
keepMovingForward = false;
stop();
}
if(key == 0) { //Turn BLE on/off
Serial.println("BLE pressed");
if(bleEnabled) { bleEnabled = false; } else { bleEnabled = true;}
}
if(key == 3) { //Turn PIXY on/off
Serial.println("PIXY pressed");
if(pixyEnabled) { pixyEnabled = false; } else { pixyEnabled = true;}
}
if(key == 4) { //Debugging
Serial.println("Debug mode");
if(debugMode) { debugMode = false; } else { debugMode = true;}
}
}
}
}
if(keepMovingForward) {
//we're interested in the PING))) but only every 1250ms
if (PINGTIMER > PINGInterval ) {
PINGTIMER -= PINGInterval; //reset the timer
//Serial.print("resetting timer="); Serial.println(PINGTIMER);
digitalWrite(13, LOW);
//Read PING Sensor value
unsigned int uS = sonar1.ping(); //send ping, get ping time in microseconds uS
unsigned int pingDistance = uS / US_ROUNDTRIP_CM; //convert time to distance
//Read SHARP IR Sensor value
uint16_t value = analogRead (SHARPpin);
// Serial.println(value); //analog value reading
double irDistance = get_IR(value); //Convert the analog voltage to the distance
if(debugMode) {
Serial.print("PING Distance (cm): "); Serial.println(pingDistance);
Serial.print("IR Distance (cm): "); Serial.println(irDistance);
}
//sometimes get false readings of 0, so avoid this with the if
if(pingDistance > triggerStopDistance && pingDistance <= triggerDetectDistance) {
// stop();
turn_right("PING detected object, move right");
} else if(pingDistance != 0 && pingDistance <= triggerStopDistance) {
//we're about to hit the wall!
callBLE("M3");
keepMovingForward = false;
stop();
}
//if(irDistance > 5 && irDistance <= 20)
if(irDistance > triggerStopDistance && irDistance <= triggerDetectDistance) {
turn_left("IR detected object, move left");
} else if(irDistance != 0 && irDistance <= triggerStopDistance) {
//we're about to hit the wall!
callBLE("M3");
keepMovingForward = false;
stop();
}
}
//allow us to not use the PIXY - could allocate to an ANALOG button trigger?
if(pixyEnabled) {
static int i = 0;
int j;
uint16_t blocks;
char buf[32];
blocks = pixy.getBlocks();
if (blocks)
{
i++;
if (i%50==0)
{
if(debugMode) { sprintf(buf, "Detected %d:\n", blocks);
Serial.print(buf); }
for (j=0; j<blocks; j++)
{
if(pixy.blocks[j].signature == 3) { //only interested in the PINK objects
if(debugMode) { sprintf(buf, " block %d: ", j);
Serial.print(buf); }
//Print out ALL the info
//pixy.blocks[j].print();
// pixy.blocks[j].x; //0 to 319
// pixy.blocks[j].y //0 to 199
uint16_t width = pixy.blocks[j].width; //1 to 320
uint16_t height = pixy.blocks[j].height; //1 to 200
if((width >= 150 && width <= 175) && (height >= 100 && height <= 135)) {
//the PINK object is about 5/10cm away and quite close to PIXY
give_chase("PINK Object is very close, let's chase it");
}
}
}
}
}
}
//keep moving forward but only if we are safe to do so
// if(keepMovingForward) {
// advance(100,100); //100 left, 100 right
// }
}
}
void callBLE(char* message) {
if(bleEnabled) {
BlueToothSerial.print(message);
}
}
void stop(void)
{
if(debugMode) { Serial.println("STOP!"); }
//need to call BLE to tell the motors to STOP
callBLE("ST");
}
void advance(char a, char b)
{
if(debugMode) { Serial.println("MOVE!"); }
//need to call BLE to tell the motors to keep moving
callBLE("MO");
}
void turn_left(char* message) {
if(debugMode) { Serial.println(message); }
callBLE("LE");
}
void turn_right(char* message) {
if(debugMode) { Serial.println(message); }
callBLE("RG");
}
void run_away(char* message) {
if(debugMode) { Serial.println(message); }
callBLE("BA");
// back_off(255,200);
// delay(500);
// stop();
}
void give_chase(char* message) {
if(debugMode) { Serial.println(message); }
callBLE("CH");
// advance(255,200);
// delay(500);
// stop();
}
//return distance (cm)
double get_IR(uint16_t value) {
//Serial.print("get_IR:");
//Serial.println(value); //255 - 262
if (value < 16) value = 16;
return 2076.0 / (value - 11.0);
}
void initializeESP8266()
{
// esp8266.begin() verifies that the ESP8266 is operational
// and sets it up for the rest of the sketch.
// It returns either true or false -- indicating whether
// communication was successul or not.
// true
int test = esp8266.begin();
if (test != true)
{
Serial.println(F("Error talking to ESP8266."));
return;
}
Serial.println(F("ESP8266 Shield Present"));
}
void connectESP8266()
{
// The ESP8266 can be set to one of three modes:
// 1 - ESP8266_MODE_STA - Station only
// 2 - ESP8266_MODE_AP - Access point only
// 3 - ESP8266_MODE_STAAP - Station/AP combo
// Use esp8266.getMode() to check which mode it's in:
int retVal = esp8266.getMode();
if (retVal != ESP8266_MODE_STA)
{ // If it's not in station mode.
// Use esp8266.setMode([mode]) to set it to a specified
// mode.
retVal = esp8266.setMode(ESP8266_MODE_STA);
if (retVal < 0)
{
Serial.println(F("Error setting mode."));
return;
}
}
Serial.println(F("Mode set to station"));
// esp8266.status() indicates the ESP8266's WiFi connect
// status.
// A return value of 1 indicates the device is already
// connected. 0 indicates disconnected. (Negative values
// equate to communication errors.)
retVal = esp8266.status();
if (retVal <= 0)
{
Serial.print(F("Connecting to "));
Serial.println(mySSID);
// esp8266.connect([ssid], [psk]) connects the ESP8266
// to a network.
// On success the connect function returns a value >0
// On fail, the function will either return:
// -1: TIMEOUT - The library has a set 30s timeout
// -3: FAIL - Couldn't connect to network.
retVal = esp8266.connect(mySSID, myPSK);
if (retVal < 0)
{
Serial.println(F("Error connecting"));
}
}
}
void displayConnectInfo()
{
char connectedSSID[24];
memset(connectedSSID, 0, 24);
// esp8266.getAP() can be used to check which AP the
// ESP8266 is connected to. It returns an error code.
// The connected AP is returned by reference as a parameter.
int retVal = esp8266.getAP(connectedSSID);
if (retVal > 0)
{
callBLE("WI");
delay(500);
Serial.print(F("Connected to: "));
Serial.println(connectedSSID);
}
// esp8266.localIP returns an IPAddress variable with the
// ESP8266's current local IP address.
IPAddress myIP = esp8266.localIP();
Serial.print(F("My IP: ")); Serial.println(myIP);
// BlueToothSerial.print("IP Address is");
// BlueToothSerial.print(myIP);
}
void clientDemo()
{
// To use the ESP8266 as a TCP client, use the
// ESP8266Client class. First, create an object:
ESP8266Client client;
// ESP8266Client connect([server], [port]) is used to
// connect to a server (const char * or IPAddress) on
// a specified port.
// Returns: 1 on success, 2 on already connected,
// negative on fail (-1=TIMEOUT, -3=FAIL).
int retVal = client.connect(destServer, 80);
if (retVal <= 0)
{
Serial.println(F("Failed to connect to server."));
return;
}
// print and write can be used to send data to a connected
// client connection.
client.print(httpRequest);
// available() will return the number of characters
// currently in the receive buffer.
while (client.available())
Serial.write(client.read()); // read() gets the FIFO char
// connected() is a boolean return value - 1 if the
// connection is active, 0 if it's closed.
if (client.connected())
client.stop(); // stop() closes a TCP connection.
}
RoMeo#2
#include <SoftwareSerial.h>
#include <SpeechSynthesis.h>
#define RxD 7
#define TxD 6
SoftwareSerial BlueToothSerial(RxD,TxD);
byte ssr[500];//define a character string
String textFromTablet;
boolean debugMode = true; //if true outputs to Serial Monitor
boolean keepMovingForward = true; //if triggered to START then move forward until told different
//Standard PWM DC control PINS //For Romeo v1.1
int E1 = 5; //M1 Speed Control
int E2 = 6; //M2 Speed Control
int M1 = 4; //M1 Direction Control
int M2 = 7; //M1 Direction Control
void setup()
{
Serial.begin(57600);
BlueToothSerial.begin(57600);
delay(500);
Serial.println("Setup completed");
advance(50,50); //start a small crawl forwards
}
void loop()
{
//constantly polling to see if we have received anything from the BLE connection
if(BlueToothSerial.available())
{
delay(10);
// Serial.print(char(BlueToothSerial.read()));
//instead of reading a char at a time, read the whole String
//but to use SpeechSynthesis.English it needs a char array
//so convert it like this
//// textFromTablet = BlueToothSerial.readString();
//// int str_len = textFromTablet.length()+1;
//// char char_array[str_len];
//// textFromTablet.toCharArray(char_array, str_len);
////Serial.print("Received:");
////Serial.println(char_array); //MOVE!LEFT!MOVE!LEFT!MOVE!LEFT!MOVE!LEFT!MOVE!LEFT!MOVE!LEFT!MOVE!LEFT!MOVE!LEFT!MOVE!LEFT!MOVE!STOP!
// saySomething(char_array);
char msg[3] = {};
int i = 0;
//read the INPUT in chunks of 2 chars
while( i < 2) { //5
msg[i] = BlueToothSerial.read();
//Serial.println(msg[i]);
++i;
}
//MO = MOVE
//ST = STOP
//LE = LEFT
//RG = RGHT
//BA = BACK
//CH = CHASE
//M1 = MSGS
//M2 = MSG0
//M3 = MSG1
//WI = WIF1
// Serial.print("REC:");Serial.println(msg);
if(strcmp(msg,"MO") == 0) {
Serial.println("msg MOVE!");
advance(100,100);
} else if(strcmp(msg,"ST") == 0) {
Serial.println("msg STOP!");
keepMovingForward = false;
stop();
saySomething("stopping");
} else if(strcmp(msg,"LE") == 0) {
Serial.println("msg LEFT!");
saySomething("turn left");
turn_L(50,100);
} else if(strcmp(msg,"RG") == 0) {
saySomething("turn right");
Serial.println("msg RGHT!");
turn_R(100,50);
} else if(strcmp(msg,"M1") == 0) {
Serial.println("Starting Robot");
saySomething("Starting robot");
} else if(strcmp(msg,"M2") == 0) {
// Serial.println("Press START button to activate robot");
saySomething("Press START button to activate robot");
} else if(strcmp(msg,"KE") == 0) {
Serial.println("START button pressed");
keepMovingForward = true;
} else if(strcmp(msg,"M3") == 0) {
keepMovingForward = false;
stop();
// Serial.println("I am about to hit something, so I will now stop");
saySomething("I am about to hit something, so I will now stop");
} else if(strcmp(msg,"WI") == 0) {
Serial.println("Connected to Weye Feye");
//saySomething("Connected to Weye Feye");
} else {
Serial.print("Data did not make sense to me...");
BlueToothSerial.read(); //it's usually due to input chars being 1 char out
Serial.println(msg);
}
}
//only useful in debugging, no use when connected to Speech Shield
// if(Serial.available())
// {
// BlueToothSerial.print(char(Serial.read()));
// }
//keep moving forward but only if we are safe to do so
if(keepMovingForward) {
advance(100,100); //100 left, 100 right
}
}
void saySomething(char* message) {
// Serial.print("SAY:");Serial.println(message);
SpeechSynthesis.buf_init(ssr);//Clear the buffer
SpeechSynthesis.English(ssr,4,"7");
SpeechSynthesis.English(ssr,6,message);
SpeechSynthesis.Espeaking(0,6,4,ssr);//Executive commands above, "0" is synthesis command; "8" select speaker; "4" speech function
while(Serial.read()!=0x41)//waiting synthesis complete
{}
while(Serial.read()!=0x4F)//waiting play complete
{}
}
void run_away(char* message) {
if(debugMode) { Serial.println(message); }
back_off(255,200);
delay(500);
stop();
}
void give_chase(char* message) {
if(debugMode) { Serial.println(message); }
advance(255,200);
delay(500);
stop();
}
void stop(void) //Stop
{
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
}
void advance(char a,char b) //Move Forward
{
analogWrite (E1,a); //PWM Speed Control
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
void back_off (char a,char b) //Move Backward
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}
void turn_L (char a,char b) //Turn Left
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
void turn_R (char a,char b) //Turn Right
{
analogWrite (E1,a);
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}
I'm sure that the code will change again and I'll probably work out a more efficient way for the BLE communication to happen, but for now, it seems to be reasonably stable.
The next task is to re-purpose a Dr.Who K9 body shell.... and see if I can either re-use the existing wheels/servo motors and somehow wedge the 2 RoMeo boards, the WiFi card and the batteries inside the K9 body. We'll see!
What I had not anticipated was the challenge of time. Or, the lack of time to spend on working on the robot. The original version was worked on towards the end of September 2015 and it was now coming up to mid-November, where had the time gone?!
I had put ArcTuRus back on the shelf, but I did have a plan in the back of my mind of how I was going to refactor a few things. For instance, I am using 2 RoMeo Arduino boards. The first was pretty much doing everything and the second was just doing speech.
The logic was that RoMeo#1 would do the sensing via the PING and IR sensor (and possibly the PIXY) and send a Bluetooth message to RoMeo#2 that would then use the Speech module to shout out loud "moving left" (or words to that effect), but RoMeo#1 would also be operating the motor servos to steer the robot away from hitting anything.
Whilst this was fine and was working okay. I still felt it was a bit unbalanced. After-all, if RoMeo#2 was being informed to "move left", "move right", "go ahead", "stop", etc... then why not connect up the motor servos to that piece of code.
Made perfect sense and seemed like a simple enough job. Which is why the thought stayed in the back of my brain not really going anywhere. It was only a matter of taking the same code from RoMeo#1 and pasting it into RoMeo#2, re-uploading and job done.
Well, in theory that was correct. In reality, not quite that simple. Hence, doing this write-up at 4am! (Well, 5am now, but you get the idea)
Initially, I ran into problems passing too much data from RoMeo#1 to RoMeo#2 via the BLE connection. The original code on RoMeo#2 was listening for any data being sent and when data was detected it would continue reading until the end of the String, before moving on and processing the String of data it was sent.
This is where flaw#1 came in. For some odd reason RoMeo#1 IR sensor was going a bit crazy, it was sending "move left" signals ever second... which as you can imagine gets a bit tiresome when all you hear from the speak is the robot noise of "move left, move left, move left....".
Looking closer at the debug output from RoMeo#2, I could see that the String contained a long string like this, MOVE,LEFT,MOVE,LEFT,MOVE,LEFT, etc...
Now, the code on RoMeo#1 was indeed doing BlueToothSerial.print() commands when the robot was meant to be moving forward (MOVE) and if it detected something with the IR sensor then it was meant to move left (LEFT) with a different BlueToothSerial.print() command. It seemed that because the transmission of the commands was within the same 10milliseconds RoMeo#2 was combining all of the messages together.
After a bit of a re-code, I changed the RoMeo#2 BLE to listen a character at a time, rather than a whole String. Sorted. Well, that's what I initially thought.
I ended up having a re-think about what was being sent from RoMeo#1 to RoMeo#2 and I thought, "why send such long text, when all I really need to send is a couple of characters to distinguish each action?". So, that is what I did.
I would just read a couple of chars that were passed over and execute the correct action, ie. send the signals to the servos to move the wheels to steer left. But, I was noticing in the debug output that the chars being received/read was sometimes getting out of sync and the code couldn't make sense of what was being received.
I was still getting the freaky 'move left' loop, which was causing the overflow of data. I added an extra line of code so that if I couldn't work out what was sent, then I must have received a dodgy character, so I needed to do an extra read() to get the code re-aligned. LovelyHack#1 It did the job and seems to be doing okay, so we'll leave that there, until it comes back to bite us when it goes into Production..... :-D
I then thought, it's probably about time to figure out why the IR Sensor is freaking out all the time and telling me to "move left". Debug mode turned on.... and I see for some odd reason the distance value that the code thinks it's detecting is about 11cm. Which is below the 20cm "object detected" setting, so it was doing the right thing, albeit there was no object at 11cm.
I repositioned the robot, having over 1m of clear space infront of it, still getting the odd readings and the "move left" speech.
More debugging.... more head scratching. Then I noticed that the WiFi card was not always connecting to the WiFi (blue status light would still be flashing), which is odd, that was always connecting okay before.
So, I did the car mechanic approach. Strip it all back to the basics and build it back up again piece by piece until you find the last piece you put on breaks it.
And there we have it...it turned out, (bizarrely!) that the PIXY camera was causing the problem.
In the code for RoMeo#1, I had a boolean flag that I was using to choose to ignore the PIXY, that was set to false, so the PIXY code wasn't getting executed. I switched it to true and, "huh?", the IR sensor was now perfectly fine and outputting the correct distance detection values.
What on earth was going on here? I fired up 'PixyMon' and plugged in the older USB cable directly to the PIXY, yep, I could see live streaming of the camera on my Mac, so the PIXY is still working.....
...oh, and then I notice it....all the settings were gone. All of my previously trained colour detections had gone.... the PIXY had "lost its memory". Probably due to the 5-6 weeks of non-usage/power supply? I had just assumed that it would keep the settings in a chip in memory or something and when power was restored it would just re-use the values, y'know, like if you download code to the Arduino and don't power the Arduino for a year, it'll still have the program inside it and it'll execute it. It won't just forget it.
I then cursed myself for not making a note of the previous PIXY setting values, as I'll have to retrain it again. But it did seem like now everything was working as it should! Phew!
In my euphoria, I also decided to repurpose some of the buttons on RoMeo#1. I had a start() and stop() button assigned, so I thought I would setup the other buttons to alter the boolean flags for bluetooth(), debugMode() and pixyEnabled().
The bluetooth() one is actually very handy as it turns off the BlueToothSerial.print() commands, which stops the speech and motor servos from being activated. Handy feature to have, from 1-4am :-D
Arduino Code
RoMeo#1
#include <NewPing.h>
#include <elapsedMillis.h>
#include <SPI.h>
#include <Pixy.h>
#include <SoftwareSerial.h>
#include <SparkFunESP8266WiFi.h>
//advance X1 speed
//:loop
// detect PING/IR Sensor values
// if PING <= 20cm stop, turnR A1
// if IR <= 20cm stop, turnL A2
// advance X2 speed
//
const char mySSID[] = "virginmedia1379311";
const char myPSK[] = "password";
const char destServer[] = "example.com";
const String httpRequest = "GET / HTTP/1.1\nHost: example.com\nConnection: close\n\n";
boolean debugMode = true; //if true outputs to Serial Monitor
boolean pixyEnabled = false; //allows ability to not use PIXY
boolean bleEnabled = true; //if true communicates over Bluetooth
boolean keepMovingForward = false;
//Standard PWM DC control PINS //For Romeo v1.1
//int E1 = 5; //M1 Speed Control
//int E2 = 6; //M2 Speed Control
//int M1 = 4; //M1 Direction Control
//int M2 = 7; //M1 Direction Control
int adc_key_val[5] ={30, 150, 360, 535, 760 };
int NUM_KEYS = 5;
int adc_key_in;
int key=-1;
int oldkey=-1;
#define RxD 3
#define TxD 2
SoftwareSerial BlueToothSerial(RxD,TxD);
#define STARTpin A2 //StartButton Pin
#define SHARPpin A1 //IRPin Analog
#define PINGInterval 1250 //milliseconds to wait for checking PING and IR Sensors
const int triggerPingPin1 = 11; //PingPin
const int maxDistance = 200; //cm
const int triggerDetectDistance = 15; //20; //cm
const int triggerStopDistance = 5; //cm
elapsedMillis PINGTIMER;
NewPing sonar1(triggerPingPin1, triggerPingPin1, maxDistance);
Pixy pixy; //doesn't mention it anywhere but I think this takes PIN 10
// Convert ADC value to key number
int get_key(unsigned int input)
{
int k;
for (k = 0; k < NUM_KEYS; k++)
{
if (input < adc_key_val[k])
{
return k;
}
}
if (k >= NUM_KEYS)
k = -1; // No valid key pressed
return k;
}
void setup(void)
{
Serial.begin(9600);
if(bleEnabled) {
BlueToothSerial.begin(38400);
}
delay(100);
callBLE("M1");
delay(1000);
if(debugMode) { Serial.println("Starting Robot"); }
//have to init the WiFi shield BEFORE anything else else it doesn't work
initializeESP8266(); //init shield
connectESP8266(); //connect to WiFi network
if(debugMode) {
displayConnectInfo(); //show the IP Address assigned
// clientDemo();
}
//init the IR Analog PIN
pinMode(SHARPpin, INPUT);
//init the PINS for the wheels
for(int i=4;i<=7;i++) {
pinMode(i, OUTPUT);
}
//init the PIXY camera
if(pixyEnabled) { pixy.init(); }
callBLE("M2");
delay(1000);
Serial.println("Started robot");
//we'll use the debug LED to output a heartbeat - just to know we're still alive
pinMode(13, OUTPUT);
}
void loop(void)
{
adc_key_in = analogRead(7); // read the value from the sensor
digitalWrite(13, HIGH);
/* get the key */
key = get_key(adc_key_in); // convert into key press
if (key != oldkey) { // if keypress is detected
delay(50); // wait for debounce time
adc_key_in = analogRead(7); // read the value from the sensor
key = get_key(adc_key_in); // convert into key press
if (key != oldkey) {
oldkey = key;
if (key >=0){
// Serial.println(adc_key_in, DEC);
Serial.print("key=");
Serial.println(key);
if(key == 1) { //up
//start the robot moving
callBLE("KE");
keepMovingForward = true;
// advance(100,100);
}
if(key == 2) { //down
//manually stop the robot
keepMovingForward = false;
stop();
}
if(key == 0) { //Turn BLE on/off
Serial.println("BLE pressed");
if(bleEnabled) { bleEnabled = false; } else { bleEnabled = true;}
}
if(key == 3) { //Turn PIXY on/off
Serial.println("PIXY pressed");
if(pixyEnabled) { pixyEnabled = false; } else { pixyEnabled = true;}
}
if(key == 4) { //Debugging
Serial.println("Debug mode");
if(debugMode) { debugMode = false; } else { debugMode = true;}
}
}
}
}
if(keepMovingForward) {
//we're interested in the PING))) but only every 1250ms
if (PINGTIMER > PINGInterval ) {
PINGTIMER -= PINGInterval; //reset the timer
//Serial.print("resetting timer="); Serial.println(PINGTIMER);
digitalWrite(13, LOW);
//Read PING Sensor value
unsigned int uS = sonar1.ping(); //send ping, get ping time in microseconds uS
unsigned int pingDistance = uS / US_ROUNDTRIP_CM; //convert time to distance
//Read SHARP IR Sensor value
uint16_t value = analogRead (SHARPpin);
// Serial.println(value); //analog value reading
double irDistance = get_IR(value); //Convert the analog voltage to the distance
if(debugMode) {
Serial.print("PING Distance (cm): "); Serial.println(pingDistance);
Serial.print("IR Distance (cm): "); Serial.println(irDistance);
}
//sometimes get false readings of 0, so avoid this with the if
if(pingDistance > triggerStopDistance && pingDistance <= triggerDetectDistance) {
// stop();
turn_right("PING detected object, move right");
} else if(pingDistance != 0 && pingDistance <= triggerStopDistance) {
//we're about to hit the wall!
callBLE("M3");
keepMovingForward = false;
stop();
}
//if(irDistance > 5 && irDistance <= 20)
if(irDistance > triggerStopDistance && irDistance <= triggerDetectDistance) {
turn_left("IR detected object, move left");
} else if(irDistance != 0 && irDistance <= triggerStopDistance) {
//we're about to hit the wall!
callBLE("M3");
keepMovingForward = false;
stop();
}
}
//allow us to not use the PIXY - could allocate to an ANALOG button trigger?
if(pixyEnabled) {
static int i = 0;
int j;
uint16_t blocks;
char buf[32];
blocks = pixy.getBlocks();
if (blocks)
{
i++;
if (i%50==0)
{
if(debugMode) { sprintf(buf, "Detected %d:\n", blocks);
Serial.print(buf); }
for (j=0; j<blocks; j++)
{
if(pixy.blocks[j].signature == 3) { //only interested in the PINK objects
if(debugMode) { sprintf(buf, " block %d: ", j);
Serial.print(buf); }
//Print out ALL the info
//pixy.blocks[j].print();
// pixy.blocks[j].x; //0 to 319
// pixy.blocks[j].y //0 to 199
uint16_t width = pixy.blocks[j].width; //1 to 320
uint16_t height = pixy.blocks[j].height; //1 to 200
if((width >= 150 && width <= 175) && (height >= 100 && height <= 135)) {
//the PINK object is about 5/10cm away and quite close to PIXY
give_chase("PINK Object is very close, let's chase it");
}
}
}
}
}
}
//keep moving forward but only if we are safe to do so
// if(keepMovingForward) {
// advance(100,100); //100 left, 100 right
// }
}
}
void callBLE(char* message) {
if(bleEnabled) {
BlueToothSerial.print(message);
}
}
void stop(void)
{
if(debugMode) { Serial.println("STOP!"); }
//need to call BLE to tell the motors to STOP
callBLE("ST");
}
void advance(char a, char b)
{
if(debugMode) { Serial.println("MOVE!"); }
//need to call BLE to tell the motors to keep moving
callBLE("MO");
}
void turn_left(char* message) {
if(debugMode) { Serial.println(message); }
callBLE("LE");
}
void turn_right(char* message) {
if(debugMode) { Serial.println(message); }
callBLE("RG");
}
void run_away(char* message) {
if(debugMode) { Serial.println(message); }
callBLE("BA");
// back_off(255,200);
// delay(500);
// stop();
}
void give_chase(char* message) {
if(debugMode) { Serial.println(message); }
callBLE("CH");
// advance(255,200);
// delay(500);
// stop();
}
//return distance (cm)
double get_IR(uint16_t value) {
//Serial.print("get_IR:");
//Serial.println(value); //255 - 262
if (value < 16) value = 16;
return 2076.0 / (value - 11.0);
}
void initializeESP8266()
{
// esp8266.begin() verifies that the ESP8266 is operational
// and sets it up for the rest of the sketch.
// It returns either true or false -- indicating whether
// communication was successul or not.
// true
int test = esp8266.begin();
if (test != true)
{
Serial.println(F("Error talking to ESP8266."));
return;
}
Serial.println(F("ESP8266 Shield Present"));
}
void connectESP8266()
{
// The ESP8266 can be set to one of three modes:
// 1 - ESP8266_MODE_STA - Station only
// 2 - ESP8266_MODE_AP - Access point only
// 3 - ESP8266_MODE_STAAP - Station/AP combo
// Use esp8266.getMode() to check which mode it's in:
int retVal = esp8266.getMode();
if (retVal != ESP8266_MODE_STA)
{ // If it's not in station mode.
// Use esp8266.setMode([mode]) to set it to a specified
// mode.
retVal = esp8266.setMode(ESP8266_MODE_STA);
if (retVal < 0)
{
Serial.println(F("Error setting mode."));
return;
}
}
Serial.println(F("Mode set to station"));
// esp8266.status() indicates the ESP8266's WiFi connect
// status.
// A return value of 1 indicates the device is already
// connected. 0 indicates disconnected. (Negative values
// equate to communication errors.)
retVal = esp8266.status();
if (retVal <= 0)
{
Serial.print(F("Connecting to "));
Serial.println(mySSID);
// esp8266.connect([ssid], [psk]) connects the ESP8266
// to a network.
// On success the connect function returns a value >0
// On fail, the function will either return:
// -1: TIMEOUT - The library has a set 30s timeout
// -3: FAIL - Couldn't connect to network.
retVal = esp8266.connect(mySSID, myPSK);
if (retVal < 0)
{
Serial.println(F("Error connecting"));
}
}
}
void displayConnectInfo()
{
char connectedSSID[24];
memset(connectedSSID, 0, 24);
// esp8266.getAP() can be used to check which AP the
// ESP8266 is connected to. It returns an error code.
// The connected AP is returned by reference as a parameter.
int retVal = esp8266.getAP(connectedSSID);
if (retVal > 0)
{
callBLE("WI");
delay(500);
Serial.print(F("Connected to: "));
Serial.println(connectedSSID);
}
// esp8266.localIP returns an IPAddress variable with the
// ESP8266's current local IP address.
IPAddress myIP = esp8266.localIP();
Serial.print(F("My IP: ")); Serial.println(myIP);
// BlueToothSerial.print("IP Address is");
// BlueToothSerial.print(myIP);
}
void clientDemo()
{
// To use the ESP8266 as a TCP client, use the
// ESP8266Client class. First, create an object:
ESP8266Client client;
// ESP8266Client connect([server], [port]) is used to
// connect to a server (const char * or IPAddress) on
// a specified port.
// Returns: 1 on success, 2 on already connected,
// negative on fail (-1=TIMEOUT, -3=FAIL).
int retVal = client.connect(destServer, 80);
if (retVal <= 0)
{
Serial.println(F("Failed to connect to server."));
return;
}
// print and write can be used to send data to a connected
// client connection.
client.print(httpRequest);
// available() will return the number of characters
// currently in the receive buffer.
while (client.available())
Serial.write(client.read()); // read() gets the FIFO char
// connected() is a boolean return value - 1 if the
// connection is active, 0 if it's closed.
if (client.connected())
client.stop(); // stop() closes a TCP connection.
}
RoMeo#2
#include <SoftwareSerial.h>
#include <SpeechSynthesis.h>
#define RxD 7
#define TxD 6
SoftwareSerial BlueToothSerial(RxD,TxD);
byte ssr[500];//define a character string
String textFromTablet;
boolean debugMode = true; //if true outputs to Serial Monitor
boolean keepMovingForward = true; //if triggered to START then move forward until told different
//Standard PWM DC control PINS //For Romeo v1.1
int E1 = 5; //M1 Speed Control
int E2 = 6; //M2 Speed Control
int M1 = 4; //M1 Direction Control
int M2 = 7; //M1 Direction Control
void setup()
{
Serial.begin(57600);
BlueToothSerial.begin(57600);
delay(500);
Serial.println("Setup completed");
advance(50,50); //start a small crawl forwards
}
void loop()
{
//constantly polling to see if we have received anything from the BLE connection
if(BlueToothSerial.available())
{
delay(10);
// Serial.print(char(BlueToothSerial.read()));
//instead of reading a char at a time, read the whole String
//but to use SpeechSynthesis.English it needs a char array
//so convert it like this
//// textFromTablet = BlueToothSerial.readString();
//// int str_len = textFromTablet.length()+1;
//// char char_array[str_len];
//// textFromTablet.toCharArray(char_array, str_len);
////Serial.print("Received:");
////Serial.println(char_array); //MOVE!LEFT!MOVE!LEFT!MOVE!LEFT!MOVE!LEFT!MOVE!LEFT!MOVE!LEFT!MOVE!LEFT!MOVE!LEFT!MOVE!LEFT!MOVE!STOP!
// saySomething(char_array);
char msg[3] = {};
int i = 0;
//read the INPUT in chunks of 2 chars
while( i < 2) { //5
msg[i] = BlueToothSerial.read();
//Serial.println(msg[i]);
++i;
}
//MO = MOVE
//ST = STOP
//LE = LEFT
//RG = RGHT
//BA = BACK
//CH = CHASE
//M1 = MSGS
//M2 = MSG0
//M3 = MSG1
//WI = WIF1
// Serial.print("REC:");Serial.println(msg);
if(strcmp(msg,"MO") == 0) {
Serial.println("msg MOVE!");
advance(100,100);
} else if(strcmp(msg,"ST") == 0) {
Serial.println("msg STOP!");
keepMovingForward = false;
stop();
saySomething("stopping");
} else if(strcmp(msg,"LE") == 0) {
Serial.println("msg LEFT!");
saySomething("turn left");
turn_L(50,100);
} else if(strcmp(msg,"RG") == 0) {
saySomething("turn right");
Serial.println("msg RGHT!");
turn_R(100,50);
} else if(strcmp(msg,"M1") == 0) {
Serial.println("Starting Robot");
saySomething("Starting robot");
} else if(strcmp(msg,"M2") == 0) {
// Serial.println("Press START button to activate robot");
saySomething("Press START button to activate robot");
} else if(strcmp(msg,"KE") == 0) {
Serial.println("START button pressed");
keepMovingForward = true;
} else if(strcmp(msg,"M3") == 0) {
keepMovingForward = false;
stop();
// Serial.println("I am about to hit something, so I will now stop");
saySomething("I am about to hit something, so I will now stop");
} else if(strcmp(msg,"WI") == 0) {
Serial.println("Connected to Weye Feye");
//saySomething("Connected to Weye Feye");
} else {
Serial.print("Data did not make sense to me...");
BlueToothSerial.read(); //it's usually due to input chars being 1 char out
Serial.println(msg);
}
}
//only useful in debugging, no use when connected to Speech Shield
// if(Serial.available())
// {
// BlueToothSerial.print(char(Serial.read()));
// }
//keep moving forward but only if we are safe to do so
if(keepMovingForward) {
advance(100,100); //100 left, 100 right
}
}
void saySomething(char* message) {
// Serial.print("SAY:");Serial.println(message);
SpeechSynthesis.buf_init(ssr);//Clear the buffer
SpeechSynthesis.English(ssr,4,"7");
SpeechSynthesis.English(ssr,6,message);
SpeechSynthesis.Espeaking(0,6,4,ssr);//Executive commands above, "0" is synthesis command; "8" select speaker; "4" speech function
while(Serial.read()!=0x41)//waiting synthesis complete
{}
while(Serial.read()!=0x4F)//waiting play complete
{}
}
void run_away(char* message) {
if(debugMode) { Serial.println(message); }
back_off(255,200);
delay(500);
stop();
}
void give_chase(char* message) {
if(debugMode) { Serial.println(message); }
advance(255,200);
delay(500);
stop();
}
void stop(void) //Stop
{
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
}
void advance(char a,char b) //Move Forward
{
analogWrite (E1,a); //PWM Speed Control
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
void back_off (char a,char b) //Move Backward
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}
void turn_L (char a,char b) //Turn Left
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
void turn_R (char a,char b) //Turn Right
{
analogWrite (E1,a);
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}
I'm sure that the code will change again and I'll probably work out a more efficient way for the BLE communication to happen, but for now, it seems to be reasonably stable.
The next task is to re-purpose a Dr.Who K9 body shell.... and see if I can either re-use the existing wheels/servo motors and somehow wedge the 2 RoMeo boards, the WiFi card and the batteries inside the K9 body. We'll see!
Comments
Post a Comment