4 Servo drive CellBot which can be remotely controlled using Arduino

I started with an idea of what I wanted to do from the beginning but one attempt after another I finally got to where I am now. This robot can be remotely controlled from anywhere in the world and driven around the house. I can see through it's eyes. Here is a brief run down. I'll get into more later.

*I have taken a 3.5 floppy disk case as a body and added 4 servo's converted to continuous operation for the locomotion.

*The hind brain if you will is an Arduino Uno Micro Controller with a sensor shield. This is used to listen to the sensors and drive the servo's to make it move.

*I'm using a Android phone, specifically the Nexus One to act as the big brain which connects via wifi to my home network. It hosts a webpage which through it I can remotely control the bot and see what it sees through the phones camera. The software to do this comes from a project called Cellbots. The supporting site for them is cellbots.com. I'm using the android app named cellbots. Are you sensing a pattern here?

*I'm using bluetooth to connect the cell phone to the Arduino.

*I'm using voltage regulator to provide enough power to charge the phone and provide power for the servos.

*I'm powering it all from a 6.6 volt LiFePO4 Battery. That's one of those new Lithium Iron batteries that will take alot of punishment.

*I'm recharging it using a Deltran 6 volt battery tender charger which I intend to set up for a automated charging station.

*I'm using a two router system port forward to the bot for the web page and a second router for all my home stuff to protect it.

While It's not actually finished, I do have enough working to prove it works as a concept. I can remotely drive it around the house. There are alot of pieces of this puzzle that probably deserve their own Instructable and I have drawn from at least one other Instructable on this site that I'll give credit to.

Step 1: Servo's and the Arduino Used to Calibrate

The arduino micro controller, model uno, is a pretty simple micro controller to use. I won't get into how to program it but I will discuss a little bit about using it in this application.

The servos I used are EXI model B1226 sourced from:
http://www.hobbypartz.com/12exiseb1.html

You will need a servo 'horn' to connect them to a wheel of some sort. This servo requires a special horn made for the B1228 model.
http://www.hobbypartz.com/sehosetforap.html

The servo is technically not a continuous servo but so incredibly easy to convert. See pictures...
1. remove cover
2. remove baring, and two gears.
3. remove little white circular clip using your finger nails.
4. calibrate the potentiometer to make it spin the same speed in both directions and to stop when you send it the command to center itself.
5. use nail polish to glue Potentiameter into place.
6. reassemble all except the little white clip you removed with your finger nails.


 Why use this specific servo?
1. Extremely high torque and just the right speed for 3 to 4 inch wheels running at 6 volts.
2. It's $14 each.
3. It already has the stops removed that prevent it from rotating continuously.
4. To make it run continuously just remove the clip. Originally it was designed to turn 8.5 times from one end to the other. After you remove the clip it will run continuously.
5. so no cutting, no soldering. If you have the calibration program on your arduino you can have it done in under 5 minutes easy.


Source code to calibrate the servo is below. It rotates right for 3 second stays still for 15 seconds, left for 3 seconds, then still for 15 seconds. You rotate the potentiameter back and forth until the thing stops spinning during the 15 second periods for several cycles of the program...

#include

Servo myservoR;


void setup() {
Serial.begin(9600);
delay(5000);
Serial.println('Start');
myservoR.attach(11);
}


void loop()
{
myservoR.write(180);
Serial.println('sending 180');
delay(3000);
myservoR.write(90);
Serial.println('sending 90');
delay(15000);
myservoR.write(0);
Serial.println('sending 0');
delay(3000);
myservoR.write(90);
Serial.println('sending 90');
delay(15000);
}

Step 2: Servo Mounts and Wheels

I used 1.5 inch x 1.5 inch L bracket from the local hardware store in my case it was Lowes. I used a hack saw to make 90 degree cuts and then pushed the area where the servo goes flat with my fingers (feel free to use tools).

The servo Horn gets pressed on and then the screw those through the enter to hold it on.

I then attach the wheels to the servo horn using screws.

The wheels are 80mm wheels from pololu.com
http://www.pololu.com/catalog/product/1430

Step 3: Attaching the Servos and Phone to Power

The servo's could be connected straight to the battery because I used a 6.6 volt power source but that still over powering them by around 20% and I wasn't happy about that so I used a voltage regulator that took the 6.6 volt in and it converted it to a fixed 5 volts.

The regulator could take a wide range of voltage in and put out the 5 volts. This is the one I used from:
http://www.pololu.com/catalog/product/2111/specs

This one requires lite soldering to assemble, took me about 2 minutes. Basically you have two wires in (battery power) and two wires out 5 volt out to power the servos and the cell phone.

The distribution block take the power off the regulator and puts it to the 4 servo's and then an extra set of wires to connect to the white breadboard.

Each of the sets of connects for the servos have 4 pins.
1 positive (on servo cable it's the red wire)
1 negative (on servo cable it's the brown wire)
1 signal from board to servo (on servo cable it's the orange wire)
1 signal from arduino to board (yellow wire)

basically the two signal pins (orange and yellow) are soldered together on the under side of the board so I can run a wire from the arduino to the board. Makes for neater safer wiring.

I am powering the phone off this voltage regulator too. basically if you find a USB cable that connects to the phone can cut it in half you will see 4 wires. You only need the red and black. They are expecting 5 volts.

Step 4: Why Connect the Phone Via Bluetooth?

The only way a Android phone can connect to the arduino via the serial (USB) port of the phone is if the phone is rooted. I didn't want to take the risk or live with the consequences of breaking into my own phone. The alternative is to connect by Bluetooth.

When selecting a Bluetooth modem I choose one that had extended range so I could use it for other projects. You don't need a class 1 (350 feet). For this application a class 2 (33 feet) will work just fine. I used the USB modem found here...

http://www.sparkfun.com/products/582

I used the following instructable on how to wire it...
https://www.instructables.com/id/how-to-Control-arduino-by-bluetooth-from-PC-pock/

Note: Whenever you need to upload you code to the arduino you need to first disconnect the positive lead of the bluetooth modem from power. If you don't the uploader will error. The bluetooth modem uses the same pins that are used by the USB connection on the Arduino.

OK, this takes care of the wiring but it doesn't tell you anything about actually using the modem. We will get to that in a later step. The thing to remember is that the communication speed (baud rate) of the modem by default from the factory comes as 115200. So you will need to change your arduino to match. I'll get into that in a later step as well.

The picture is borrowed from the manufacturer because mine is buried too deep to show.

While the modem has 6 wiring points you only need 4 for the arduino - +5v, Ground, RX (receive), TX (transmit). The Arduino pins 1 and 2 have RX and TX on them as well. The wiring is reversed so that the receiving side listens to the transmitting side RX to TX and TX to RX. I recommend either using 22 gauge solid core copper wire for the leads, so they can be pressed into the arduino, or wiring in some male leads.

For the sake of clarity, the phone and the modem both ride on the bot so the distance between them is very short.

Step 5: Choice of Power Source

I originally chose to use a LiFePO4 battery. That's one of those lithium Iron batteries that can be charged in like 5 minutes. I chose it because this specific one has 2 cells in series bringing the power to 6.6 volts which is just high enough for the arduino built in voltage regulator which steps down (converts) to 5 volts but not so high that the 6 volt servo's couldn't run off of it. It's nearly impossible to over charge the battery. It doesn't out-gas so no worries of blowing up.

While it worked, I at one point was getting some odd behavior from the servos that I couldn't identify another source so I switched to voltage regulation down to 5 volts for the servos. This turned out to be a good decision as it allowed me to provide power to the phone.

The LiFePO4 batterys have a extremely flat drain curve so as the battery is drained the voltage doesn't drop much until it's below 20% power.

Chargers for these type of batteries can be a little hard to come by but I found that the 6 volt 1.25 amp battery tender charger (for old motorcycles or cars) made by Deltran works well. It stops charging at 7.2 volts which is just the right stopping voltage (compared to my other charger). The charger also turns on as soon as contact is made. It auto stops. It even has a built in float charger. Just make sure you buy this one specifically because if you buy one with the wrong stop voltage or buy one with desulfating technology you will ruin your battery. This float charger by spec's only kicks in around 10% charge which is a little low for my taste, but then it wasn't made for this battery.

You can use this charger later for an automated charging station, which I haven't finished.

One Nice touch which came later, a 3 position switch was added. One position turns the bot on, middle turns bot off, Last position connects the battery to a couple of charging terminals for recharge while disconnected from bot. To wire find a three position switch with 3 connectors on the bottom do the following. Connect positive power to the middle pin and then one outer pin goes to a positive charging terminal and the other pin goes to Bot Power.

link to battery:
http://www.hobbypartz.com/a123-systems-4600mah-6-6v-2s2p.html

link to battery connector, I bought my Deans Ultra connector from a hobby store but if you buy this one you can just cut the white end off and attach the two ends to a wiring block:
http://www.amazon.com/Pro-Boat-Tamiya-Charge-Adapter/dp/B000SC6R5C/ref=sr_1_24?s=toys-and-games&ie=UTF8&qid=1310661997&sr=1-24

link to charger:
http://www.amazon.com/Deltran-Battery-Tender-Plus-Volt/dp/B004V9FD58/ref=sr_1_29?ie=UTF8&qid=1310660649&sr=8-29

Step 6: The Cellbots Phone App and Setup

I'm using a Android app called Cellbots from Cellbots.com which can be obtained through the Android marketplace. It offers multiple configurations. Here are directions for setting up...

0. turn on your bluetooth modem. (provide power regardless of it it's connect to anything).
1. open app.
2. click continue button for description.
3. Click cellbot button.
4. select add new cellbot
5. a. give bot name
b. cellbot type is default controller
c. Cellbot bluetooth is FireFly ( if you used the BlueSmirf modem).
d. Click Done
6. Click the back button to go to a list of bots.
7. Click and hold on the button for the new bot.
8. when menu comes up click Share
9. scroll down to Communication Method and from the menu select Custom HTTP.
10. Scroll down to Cellbot's URL and look for a checkbox labeled Use Local Server, check it to the on position.
11. Note that if the phone is connect through 3G or 4G cell data, it will be using you the IP address of the cell network. I have only tested it with wifi turned on and cell service turned off (sim is actually out of the phone). Copy the HTTP address down. This is what you will browse to, to control your bot.

If you are using your cell connection you are done.

If you are using wifi then you have more to do.
1. You will need to log into your router, and go into your port forwarding section, and forward port 8080 traffic to the IP address that was listed on the phone. (note any port forwarding makes a network more vulnerable to be hacked so do at your own risk. To reduce this risk for myself, I connected my wifi phone to my home Fios (internet provider) router/modem combo and then added a second router behind the first to connect the rest of my home network, so, if the phone is hacked, they still have to get past the second router to get into the network. )
2. Go to http://www.whatismyip.com/ to get your routers IP address.
3. Add the port number of 8080 to that address and Copy the HTTP address down. This is what you will use to go to, to control your bot. Example take 50.99.99.99 and convert to http://50.99.99.99:8080/ Use this to visit your bot.

More instructions will be given in a later step to actually set up and use.

Step 7: The Arduino Code (unmodified)

/*
  Servo driven robot commanded by serial input

 Looks for a set of ASCII characters in the signal to send
 commands to a set of servos to drive a small robot. LED pin #13
 will remain lit during servo movement and blink for speed changes.


 The minimum circuit:
 * LED attached from pin 13 to ground (or use built-in LED on most Arduino's)
 * Servos with signal wires connected to pins 3 and 5 (5v power and ground for
 servos can also be wired into Arduino, or power can come from external source)
 * Serial input connected to RX pin 0
 * Serial output connected to TX pin 1

 Additional circuits (optional):
 * Forward facing ultrasonic range finder on digital pin 7
 * Downward facing ultrasonic range finder on digital pin 8

 Note: If you don't yet have a serial device to connect with, you can use the
built in Serial Monitor in the Arduino software when connect via USB for testing.
Also, be sure to disconect RX & TX pins from other devices when trying to program
the Arduino over USB.

created 2010
by Tim Heath, Ryan Hickman, and Glen Arrowsmith
Visit http://www.cellbots.com for more information
*/

#include <Servo.h>
#include <EEPROM.h>

#define BUFFERSIZE 20
#define EEPROM_servoCenterLeft 1
#define EEPROM_servoCenterRight 2
#define EEPROM_speedMultiplier 3
#define EEPROM_servosForcedActive 4
#define EEPROM_lastNeckValue 5

#define DEFAULT_servoCenterLeft 90
#define DEFAULT_servoCenterRight 90
#define DEFAULT_speedMultiplier 5
#define DEFAULT_servosForcedActive false
#define DEFAULT_servosForcedActive false
#define DEFAULT_lastNeckValue 255

// ** GENERAL SETTINGS ** - General preference settings
boolean DEBUGGING = false; // Whether debugging output over serial is on by defauly (can be flipped with 'h' command)
const int ledPin = 13; // LED turns on while running servos
char* driveType = "servo"; // Use "motor" when bots has a DC motor driver or "servo" for servos powering the wheels

// ** SERVO SETTINGS ** - Configurable values based on pins used and servo direction
const int servoPinLeft = 9;
const int servoPinRight = 10;
const int servoPinHead = 12; // Servo controlling the angle of the phone
const int servoDirectionLeft = 1; // Use either 1 or -1 for reverse
const int servoDirectionRight = -1; // Use either 1 or -1 for reverse
int servoCenterLeft = DEFAULT_servoCenterLeft; // PWM setting for no movement on left servo
int servoCenterRight = DEFAULT_servoCenterLeft; // PWM setting for no movement on right servo
int servoPowerRange = 30; // PWM range off of center that servos respond best to (set to 30 to work in the 60-120 range off center of 90)
const long maxRunTime = 2000; // Maximum run time for servos without additional command. * Should use a command to set this. *
int speedMultiplier = DEFAULT_speedMultiplier; // Default speed setting. Uses a range from 1-10
int lastNeckValue = DEFAULT_lastNeckValue;

// ** MOTOR DRIVER SETTINGS ** - For use with boards like the Pololu motor driver (also uses left/right servo pin settings above)
int leftMotorPin_1 = 9;
int leftMotorPin_2 = 8;
int rightMotorPin_1 = 10;
int rightMotorPin_2 = 11;
int motor_stby = 12;

// ** RANGE FINDING *** - The following settings are for ultrasonic range finders. OK to lave as-is if you don't have them on your robot
long dist, microseconds, cm, inches; // Used by the range finder for calculating distances
const int rangePinForward = 7; // Digital pin for the forward facing range finder (for object distance in front of bot)
const int rangeToObjectMargin = 0; // Range in cm to forward object (bot will stop when distance closer than this - set to 0 if no sensor)
const int rangePinForwardGround = 8; // Digital pin for downward facing range finder on the front (for edge of table detection)
const int rangeToGroundMargin = 0; // Range in cm to the table (bot will stop when distance is greater than this set to 0 if no sensor)
const int rangeSampleCount = 3; // Number of range readings to take and average for a more stable value

// Create servo objects to control the servos
Servo myservoLeft;
Servo myservoRight;
Servo myservoHead;

// No config required for these parameters
boolean servosActive = false; // assume servos are not moving when we begin
boolean servosForcedActive = DEFAULT_servosForcedActive; // will only stop when considered dangerous
unsigned long stopTime=millis(); // used for calculating the run time for servos
char incomingByte; // Holds incoming serial values
char msg[8]; // For passing back serial messages
char inBytes[BUFFERSIZE]; //Buffer for serial in messages
int serialIndex = 0;
int serialAvail = 0;

void setup() {
pinMode(servoPinLeft, OUTPUT);
pinMode(servoPinRight, OUTPUT);
pinMode(servoPinHead, OUTPUT);
pinMode(leftMotorPin_1,OUTPUT);
pinMode(leftMotorPin_2,OUTPUT);
pinMode(rightMotorPin_1,OUTPUT);
pinMode(rightMotorPin_2,OUTPUT);
pinMode(ledPin, OUTPUT);
digitalWrite(servoPinLeft,0);
digitalWrite(servoPinRight,0);
digitalWrite(servoPinHead,0);
digitalWrite(motor_stby,HIGH);
Serial.begin(115200);
servoCenterLeft = readSetting(EEPROM_servoCenterLeft, servoCenterLeft);
servoCenterRight = readSetting(EEPROM_servoCenterRight, servoCenterRight);
speedMultiplier = readSetting(EEPROM_speedMultiplier, speedMultiplier);
servosForcedActive = readSetting(EEPROM_servosForcedActive, servosForcedActive);
lastNeckValue = readSetting(EEPROM_lastNeckValue, lastNeckValue);
if (lastNeckValue != DEFAULT_lastNeckValue) {
myservoHead.attach(servoPinHead);
myservoHead.write(lastNeckValue);
}
}

//Safely reads EEPROM
int readSetting(int memoryLocation, int defaultValue) {
int value = EEPROM.read(memoryLocation);
if (value == 255) {
EEPROM.write(memoryLocation, defaultValue);
}
return value;
}

//Sets the EEPROM settings to the default values
void setEepromsToDefault() {
servosForcedActive = DEFAULT_servosForcedActive;
speedMultiplier = DEFAULT_speedMultiplier;
servoCenterRight = DEFAULT_servoCenterRight;
servoCenterLeft = DEFAULT_servoCenterLeft;
lastNeckValue = DEFAULT_lastNeckValue;
EEPROM.write(EEPROM_servosForcedActive, DEFAULT_servosForcedActive);
EEPROM.write(EEPROM_speedMultiplier, DEFAULT_speedMultiplier);
EEPROM.write(EEPROM_servoCenterRight, DEFAULT_servoCenterRight);
EEPROM.write(EEPROM_servoCenterLeft, DEFAULT_servoCenterLeft);
EEPROM.write(EEPROM_lastNeckValue, DEFAULT_lastNeckValue);
if (DEBUGGING) {
Serial.println("All EEPROM values set to defaults.");
}
}

// Convert directional text commands ("forward"/"backward") into calculated servo speed
int directionValue(char* directionCommand, int servoDirection) {
if (directionCommand == "forward") {
return (10 * speedMultiplier * servoDirection);
}
else if (directionCommand == "backward") {
return (-10 * speedMultiplier * servoDirection);
}
else {
if (DEBUGGING) { Serial.println("Houston, we have a problem!"); }
return 0; // Attemp to set value to center - this shouldn't be needed
}
}

// Translate text commands into PWM values for the bot to move (left servo command, right servo command)
unsigned long moveBot(char* commandLeft, char* commandRight) {
int valueLeft = directionValue(commandLeft, servoDirectionLeft) + servoCenterLeft;
int valueRight = directionValue(commandRight, servoDirectionRight) + servoCenterRight;
driveWheels(valueLeft, valueRight);
}


// Drive servo or DC motors to move the robot using values in range -100 to 100 for left and right
unsigned long driveWheels(int valueLeft, int valueRight) {
// Detach both servo pins which will stop whine and de-energize the motors so they don't kill the compass readings
if (valueLeft == 0 and valueRight == 0){
myservoLeft.detach();
myservoRight.detach();
}
// Drive the wheels based on "servo" driveType
if (driveType == "servo"){
valueLeft = valueLeft * servoDirectionLeft; // Flip positive to negative if needed based on servo direction value setting
valueRight = valueRight * servoDirectionRight;
// Map "w" values to the narrow range that the servos respond to
valueLeft = map(valueLeft, -100, 100, (servoCenterLeft - servoPowerRange), (servoCenterLeft + servoPowerRange));
valueRight = map(valueRight, -100, 100, (servoCenterRight - servoPowerRange), (servoCenterRight + servoPowerRange));
digitalWrite(ledPin, HIGH); // set the LED on
// Restart the servo PWM and send them commands
myservoLeft.attach(servoPinLeft);
myservoRight.attach(servoPinRight);
myservoLeft.write(valueLeft);
myservoRight.write(valueRight);
// Spit out some diagnosis info over serial
if (DEBUGGING) {
Serial.print("Moving left servo ");
Serial.print(valueLeft, DEC);
Serial.print(" and right servo ");
Serial.println(valueRight, DEC);
}
}
// Drive the wheels based on "motor" driveType
else{
// Set left motor pins to turn in the desired direction
if (valueLeft < 0){
digitalWrite(leftMotorPin_1,LOW);
digitalWrite(leftMotorPin_2,HIGH);
}
else {
digitalWrite(leftMotorPin_1,HIGH);
digitalWrite(leftMotorPin_2,LOW);
}
// Set right motor pins to turn in the desired direction
if (valueRight < 0){
digitalWrite(rightMotorPin_1,LOW);
digitalWrite(rightMotorPin_2,HIGH);
}
else {
digitalWrite(rightMotorPin_1,HIGH);
digitalWrite(rightMotorPin_2,LOW);
}
// Maps "w" values to the wider range that the motor responds to
valueLeft = map(abs(valueLeft), 0, 100, 0, 255);
valueRight = map(abs(valueRight), 0, 100, 0, 255);
analogWrite(servoPinLeft,valueLeft);
analogWrite(servoPinRight,valueRight);
}

stopTime=millis() + maxRunTime; // Set time to stop running based on allowable running time
return stopTime;
}

// Stop the bot
void stopBot() {
driveWheels(0,0);
digitalWrite(ledPin, LOW); // Turn the LED off
if (DEBUGGING) { Serial.println("Stopping both wheels"); }
serialReply("i", "st"); // Tell the phone that the robot stopped
}

// Read and process the values from an ultrasonic range finder (you can leave this code in even if you don't have one)
long getDistanceSensor(int ultrasonicPin) {
// Take multiple readings and average them
microseconds = 0;
for(int sample = 1 ; sample <= rangeSampleCount; sample ++) {
// The Parallax PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
// The Maxsonar does not seem to need this part but it does not hurt either
pinMode(ultrasonicPin, OUTPUT);
digitalWrite(ultrasonicPin, LOW);
delayMicroseconds(2);
digitalWrite(ultrasonicPin, HIGH);
delayMicroseconds(5);
digitalWrite(ultrasonicPin, LOW);

// The same pin is used to read the signal from the ultrasonic detector: a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(ultrasonicPin, INPUT);
microseconds += pulseIn(ultrasonicPin, HIGH);
delayMicroseconds(5); // Very short pause between readings
}
microseconds = microseconds / rangeSampleCount;
// Convert the averaged sensor reading to centimeters and return it
cm = microsecondsToCentimeters(microseconds);
inches = microsecondsToInches(microseconds);
if (DEBUGGING) {
Serial.print("Micro: "); Serial.print(microseconds);
Serial.print(" Inches: "); Serial.print(inches);
Serial.print(" cm: "); Serial.println(cm);
}
return cm;
}

long microsecondsToCentimeters(long microseconds) {
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
return microseconds / 29 / 2;
}

long microsecondsToInches(long microseconds) {
// According to Parallax's datasheet for the PING))), there are
// 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
// second). This gives the distance travelled by the ping, outbound
// and return, so we divide by 2 to get the distance of the obstacle.
// See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
// Same is true for the MaxSonar by MaxBotix
return microseconds / 74 / 2;
}

// Replies out over serial and handles pausing and flushing the data to deal with Android serial comms
void serialReply(char* sensorname, char* tmpmsg) {
Serial.print(sensorname);
Serial.print(":");
Serial.println(tmpmsg); // Send the message back out the serial line
//Wait for the serial debugger to shut up
delay(200); //this is a magic number
Serial.flush(); //clears all incoming data
}

// Checks range finders to see if it is safe to continue moving (* need to add way to know which direction we're moving *)
boolean safeToProceed(){
boolean safe = false; // Assume it isn't safe to proceed
// Check the distance to the nearest object in front of the bot and stop if too close
if (rangeToObjectMargin != 0){ // Don't bother sending if margin set to zero because it hangs when no sensor present
dist = getDistanceSensor(rangePinForward);
if (dist > rangeToObjectMargin) {
safe = true;
}
else if (DEBUGGING) {Serial.print("Object too close in front - ");}
}
// Check the distance to the ground in front of the bot to make sure the table is still there
if (rangeToGroundMargin != 0){ // Don't bother sending if margin set to zero because it hangs when no sensor present
dist = getDistanceSensor(rangePinForwardGround);
if (dist > rangeToGroundMargin) {
safe = true;
}
else if (DEBUGGING) {Serial.print("End of surface reached - ");}
}
if (rangeToGroundMargin == 0 && rangeToObjectMargin == 0) {return true;}
return safe;
}

// Check if enough time has elapsed to stop the bot and if it is safe to proceed
void checkIfStopBot() {
if (not servosForcedActive and servosActive and (stopTime < millis() or not safeToProceed())) {
stopBot();
servosActive = false;
} else if (not safeToProceed()) {
stopBot();
servosActive = false;
}
}

// Send command to attached Bluetooth device to initiate pairing
void pairBluetooth() {
Serial.print("\r\n+INQ=1\r\n"); // This is for Seeedstudio master/slave unit (change as needed for your model)
}

// Reads serial input if available and parses command when full command has been sent.
void readSerialInput() {
serialAvail = Serial.available();
//Read what is available
for (int i = 0; i < serialAvail; i++) {
//Store into buffer.
inBytes[i + serialIndex] = Serial.read();
//Check for command end.

if (inBytes[i + serialIndex] == '\n' || inBytes[i + serialIndex] == ';' || inBytes[i + serialIndex] == '>') { //Use ; when using Serial Monitor
inBytes[i + serialIndex] = '\0'; //end of string char
parseCommand(inBytes);
serialIndex = 0;
}
else {
//expecting more of the command to come later.
serialIndex += serialAvail;
}
}
}

// Cleans and parses the command
void parseCommand(char* com) {
if (com[0] == '\0') { return; } //bit of error checking
int start = 0;
//get start of command
while (com[start] != '<'){
start++;
if (com[start] == '\0') {
//its not there. Must be old version
start = -1;
break;
}
}
start++;
//Shift to beginning
int i = 0;
while (com[i + start - 1] != '\0') {
com[i] = com[start + i];
i++;
}
performCommand(com);
}

void performCommand(char* com) {
if (strcmp(com, "f") == 0) { // Forward
stopTime = driveWheels(speedMultiplier * 10, speedMultiplier * 10);
servosActive = true;
} else if (strcmp(com, "r") == 0) { // Right
stopTime = driveWheels(speedMultiplier * 10, speedMultiplier * -10);
servosActive = true;
} else if (strcmp(com, "l") == 0) { // Left
stopTime = driveWheels(speedMultiplier * -10, speedMultiplier * 10);
servosActive = true;
} else if (strcmp(com, "b") == 0) { // Backward
stopTime = driveWheels(speedMultiplier * -10, speedMultiplier * -10);
servosActive = true;
} else if (strcmp(com, "s") == 0) { // Stop
stopBot();
servosActive = false;
} else if (strcmp(com, "fr") == 0 || strcmp(com, "fz") == 0 || strcmp(com, "x") == 0) { // Read and print forward facing distance sensor
dist = getDistanceSensor(rangePinForward);
itoa(dist, msg, 10); // Turn the dist int into a char
serialReply("x", msg); // Send the distance out the serial line
} else if (strcmp(com, "z") == 0) { // Read and print ground facing distance sensor
dist = getDistanceSensor(rangePinForwardGround);
itoa(dist, msg, 10); // Turn the dist int into a char
serialReply("z", msg); // Send the distance out the serial line
} else if (strcmp(com, "h") == 0) { // Help mode - debugging toggle
// Print out some basic instructions when first turning on debugging
if (not DEBUGGING) {
Serial.println("Ready to listen to commands! Try ome of these:");
Serial.println("F (forward), B (backward), L (left), R (right), S (stop), D (demo).");
Serial.println("Also use numbers 1-9 to adjust speed (0=slow, 9=fast).");
}
DEBUGGING = !DEBUGGING;
} else if (strcmp(com, "1") == 0 || strcmp(com, "2") == 0 || strcmp(com, "3") == 0 || strcmp(com, "4") == 0 || strcmp(com, "5") == 0 || strcmp(com, "6") == 0 || strcmp(com, "7") == 0 || strcmp(com, "8") == 0 || strcmp(com, "9") == 0 || strcmp(com, "0") == 0) {
//I know the preceeding condition is dodgy but it will change soon
if (DEBUGGING) { Serial.print("Changing speed to "); }
int i = com[0];
speedMultiplier = i - 48; // Set the speed multiplier to a range 1-10 from ASCII inputs 0-9
EEPROM.write(EEPROM_speedMultiplier, speedMultiplier);
if (DEBUGGING) { Serial.println(speedMultiplier); }
// Blink the LED to confirm the new speed setting
for(int speedBlink = 1 ; speedBlink <= speedMultiplier; speedBlink ++) {
digitalWrite(ledPin, HIGH); // set the LED on
delay(100);
digitalWrite(ledPin, LOW); // set the LED off
delay(100);
}
} else if (com[0] == 'c') { // Calibrate center PWM settings for both servos ex: "c 90 90"
int valueLeft=90, valueRight=90;
sscanf (com,"c %d %d",&valueLeft, &valueRight); // Parse the input into multiple values
servoCenterLeft = valueLeft;
servoCenterRight = valueRight;
stopTime = driveWheels(0,0); // Drive the servos with 0 value which should result in no movement when calibrated correctly
servosActive = true;
EEPROM.write(EEPROM_servoCenterLeft, servoCenterLeft);
EEPROM.write(EEPROM_servoCenterRight, servoCenterRight);
if (DEBUGGING) {
Serial.print("Calibrated servo centers to ");
Serial.print(servoCenterLeft);
Serial.print(" and ");
Serial.println(servoCenterRight);
}
} else if (strcmp(com, "i") == 0) { // Toggle servo to infinite active mode so it doesn't time out automatically
servosForcedActive = !servosForcedActive; // Stop only when dangerous
EEPROM.write(EEPROM_servosForcedActive, servosForcedActive);
if (DEBUGGING) {
Serial.print("Infinite rotation toggled to ");
if (servosForcedActive){Serial.println("on");}
else {Serial.println("off");}
}
} else if (com[0] == 'w') { // Handle "wheel" command and translate into PWM values ex: "w -100 100" [range is from -100 to 100]
int valueLeft=90, valueRight=90;
sscanf (com,"w %d %d",&valueLeft, &valueRight); // Parse the input into multiple values
stopTime = driveWheels(valueLeft, valueRight);
servosActive = true;
} else if (strcmp(com, "reset") == 0) { // Resets the eeprom settings
setEepromsToDefault();
} else if (com[0] == 'n') { // Move head up
sscanf (com,"n %d",&lastNeckValue); // Parse the input into multiple values
myservoHead.attach(servoPinHead);
myservoHead.write(lastNeckValue);
EEPROM.write(EEPROM_lastNeckValue, lastNeckValue);
if (DEBUGGING) {
Serial.print("Neck moved to ");
Serial.println(lastNeckValue);
}
} else if (com[0] == 'p') { // Initiates Bluetooth pairing so another device can connect
pairBluetooth();
} else {
serialReply("e", com);// Echo unknown command back
if (DEBUGGING) {
Serial.print("Unknown command: ");
Serial.println(com);
}
}
}

// Main loop running at all times
void loop()
{
readSerialInput();
checkIfStopBot();
}

Step 8: The Arduino Code Adapted to My Use.

//Cellbots code base rewritten.


#include <Servo.h>
#include <EEPROM.h>

Servo myservoFR;
Servo myservoFL;
Servo myservoRR;
Servo myservoRL;
Servo myservoHead;

#define BUFFERSIZE 20
#define InferFPin 0 //Front Inferred Sensor analog pin
#define InferBPin 1 //Back Inferred Sensor analog pin
#define SonarPin 2 //sonar analog pin
#define LeftEarPin 3 //left ear analog pin
#define RightEarPin 4 //right ear analog pin/
#define VoltPin 5 //analog pin 5 used for VoltageCheck detection.
#define LEDBlue 5
#define LEDGreen 6
#define LEDRed 7
#define servoPinHead 8 // Servo controlling the angle of the phone
#define FLpin 9
#define FRpin 10
#define RLpin 11
#define RRpin 12
#define ledPin 13 // LED turns on while running servos
#define regulator 20

//#define OneDeg 5.65 //setting rotational delay for equal to one degree (2 wheel carpet)
#define OneDeg 9.04 //setting rotational delay for equal to one degree (2 wheel hardwood)
// Checking battery voltage turned out to be the hardest thing I attempted.  Having both battery connected and USB connected...
// gives different readings then just the battery connected.  In fact the analog readings were reversed somehow.
#define shutdownVoltageCheck 978   // this number is unique to every voltage divider and battery combo.  It's basically 20% of mine.
#define analogvoltconstant 144.6969

int iAnVal; // sonar sensor input
int deg;
int InferF=0;
int InferB=0;
int soundRT[72];
int soundLF[72];
int distance[72];
int tempvolt;
unsigned long BatteryIndex = 0;
unsigned long BatteryIndexThreshold= 150000; // about 10 minutes.
unsigned long RegulatorIndex;
unsigned long RegulatorTimerThreshold= 150000; // interval turn on or turn off the voltage regulator. about 10 minutes.
int VoltSampleSize=1000;
char dir ='s'; // zero's the direction the bot has been told to drive - forward, backward, right, left, stop, sets it to stop.
int power =-1;
boolean ConservePower = true; //this determines if the bot uses power saving mode.
int warmup = 1000; //delay to start setup in Milliseconds.

// No config required for these parameters
boolean servosActive = false; // assume servos are not moving when we begin
// unsigned long stopRegulatorIndex=millis(); // used for calculating the run RegulatorIndex for servos
char incomingByte; // Holds incoming serial values
char msg[8]; // For passing back serial messages
char inBytes[BUFFERSIZE]; //Buffer for serial in messages
int serialIndex = 0;
int serialAvail = 0;

// ** RANGE FINDING *** -  settings are for ultrasonic range finders. OK to lave as-is if you don't have them on your robot
long dist, microseconds, cm, inches; // Used by the range finder for calculating distances
const int rangePinForward = 2; // Digital pin for the forward facing range finder (for object distance in front of bot)
const int rangeToObjectMargin = 25; //(bot will stop when distance closer than this - set to 0 if no sensor)
const int rangePinForwardGround = 0; // Digital pin for downward facing range finder on the front (for edge of table detection)
const int rangeToGroundMargin = 0; // Range in cm to the table (bot will stop when distance is greater than this set to 0 if no sensor)
const int rangeSampleCount = 3; // Number of range readings to take and for a more stable value





//=============================================================================
void setup() {

Serial.println("start setup!");
delay(warmup);

//********************** set input output mode *****************************
pinMode(LEDBlue, OUTPUT);
pinMode(LEDGreen, OUTPUT);
pinMode(LEDRed, OUTPUT);
pinMode(servoPinHead, OUTPUT);
pinMode(FLpin,OUTPUT);
pinMode(FRpin,OUTPUT);
pinMode(RLpin,OUTPUT);
pinMode(RRpin,OUTPUT);
pinMode(ledPin, OUTPUT);
pinMode(regulator, OUTPUT);




// digitalWrite(servoPinHead,0);

Serial.begin(9600);
Serial3.begin(115200);

// lastNeckValue = readSetting(EEPROM_lastNeckValue, lastNeckValue);
// if (lastNeckValue != DEFAULT_lastNeckValue) {
// myservoHead.attach(servoPinHead);
// myservoHead.write(lastNeckValue);
// }



//***************************** Clear Sensor Map ************************
for (deg = 0; deg < 72; deg = deg + 1) {
soundRT[deg]=0;
soundLF[deg]=0;
distance[deg]=0;
}

//***************************** default to servos turned off *************
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();

LEDOff();

pinMode(regulator, OUTPUT); // set pin 20 to output mode so the VoltageCheck regulator can be turned on and off.
digitalWrite(regulator, LOW); // start the VoltageCheck regulator in the off position.

Serial.println("end setup!");

}
//====================================================================================




// Replies out over serial and handles pausing and flushing the data to deal with Android serial comms
void serialReply(char* sensorname, char* tmpmsg) {
Serial3.print(sensorname);
Serial3.print(":");
Serial3.println(tmpmsg); // Send the message back out the serial line
//Wait for the serial debugger to shut up
delay(200); //this is a magic number
Serial3.flush(); //clears all incoming data
}





// Check if enough RegulatorIndex has elapsed to stop the bot and if it is safe to proceed
boolean checkIfStopBot() {
int tempfrontsonar = Averagefive(SonarPin);
//Serial.print("front sonar: ");
//Serial.println(tempfrontsonar);
int tempfrontinfer = Averagefive(InferFPin);
//Serial.print("front infer: ");
//Serial.println(tempfrontinfer);
int tempbackinfer = Averagefive(InferBPin);
//Serial.print("back infer: ");
//Serial.println(tempbackinfer);
//Serial.println();

if ((dir=='f') && ((tempfrontsonar < rangeToObjectMargin) || (tempfrontinfer < 200))) {
Stop();
servosActive = false;
return true;
} else if (dir=='b') {
if (!((tempbackinfer < 650) && (tempbackinfer > 450))) {
Stop();
servosActive = false;
return true;
}
}
return false;
}




// Send command to attached Bluetooth device to initiate pairing
void pairBluetooth() {
Serial3.print("\r\n+INQ=1\r\n"); // This is for Seeedstudio master/slave unit (change as needed for your model)
}




// Reads serial input if available and parses command when full command has been sent.
void readSerialInput() {
serialAvail = Serial3.available();
//Read what is available
for (int i = 0; i < serialAvail; i++) {
//Store into buffer.
inBytes[i + serialIndex] = Serial3.read();
//Check for command end.

if (inBytes[i + serialIndex] == '\n' || inBytes[i + serialIndex] == ';' || inBytes[i + serialIndex] == '>') { //Use ; when using Serial Monitor
inBytes[i + serialIndex] = '\0'; //end of string char
parseCommand(inBytes);
serialIndex = 0;
}
else {
//expecting more of the command to come later.
serialIndex += serialAvail;
}
}
}




// Cleans and parses the command
void parseCommand(char* com) {
if (com[0] == '\0') { return; } //bit of error checking
int start = 0;
//get start of command
while (com[start] != '<'){
start++;
if (com[start] == '\0') {
//its not there. Must be old version
start = -1;
break;
}
}
start++;
//Shift to beginning
int i = 0;
while (com[i + start - 1] != '\0') {
com[i] = com[start + i];
i++;
}
performCommand(com);
}





void LEDWhite() {
digitalWrite(7, LOW);
digitalWrite(6, LOW);
digitalWrite(5, LOW);
}




void LEDOff() {
digitalWrite(7, HIGH);
digitalWrite(6, HIGH);
digitalWrite(5, HIGH);
}




void Detach() {
//Serial.println("detach servos!");
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();
// delay(150);
}




void Attach() {
//Serial.println("Attach servos!");
myservoFR.attach(FRpin);
myservoFL.attach(FLpin);
myservoRR.attach(RRpin);
myservoRL.attach(RLpin);
digitalWrite(regulator, HIGH);
}




void Forew() {
//Serial.println("*** Forward!");
dir='f';
if (not checkIfStopBot()) {
Attach();
myservoFR.write(180);
myservoFL.write(0);
myservoRR.write(180);
myservoRL.write(0);
RegulatorIndex=0;
servosActive = true;
}
}




void Backw () {
//int Tempdist = Averagefive(InferFPin);
//Serial.print("backward distance: ");
//Serial.println(Tempdist);
//Serial.println();
dir='b';
if (not checkIfStopBot()){
//Serial.println("*** Backward!");
Attach();
myservoFR.write(0);
myservoFL.write(180);
myservoRR.write(0);
myservoRL.write(180);
RegulatorIndex=0;
servosActive = true;
}
}




void Stop () {
Serial.println("*** Stop!");
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();
dir='s';
RegulatorIndex=0;
serialReply("i", "st"); // Tell the phone that the robot stopped ###
servosActive = false;
}




void Left () {
Serial.println("*** Rotate Left!");
Attach();
myservoFR.write(180);
myservoFL.write(180);
myservoRR.write(180);
myservoRL.write(180);
dir='l';
RegulatorIndex=0;
servosActive = true;
}




void Right () {
Serial.println("*** Rotate Right!");
Attach();
myservoFR.write(0);
myservoFL.write(0);
myservoRR.write(0);
myservoRL.write(0);
dir='r';
RegulatorIndex=0;
servosActive = true;
}




void CircleRt () {
Serial.println("*** Circle Right!");
Attach();
myservoFR.write(95);
myservoFL.write(0);
myservoRR.write(95);
myservoRL.write(0);
RegulatorIndex=0;
}




void CircleLf () {
Serial.println("*** Circle Left!");
Attach();
myservoFR.write(180);
myservoFL.write(85);
myservoRR.write(180);
myservoRL.write(85);
RegulatorIndex=0;
}




void mydelay (int degress) {
//Serial.println("mydelay start");
delay (int(OneDeg * degress));
//Serial.println("mydelay end");
}




int Averagefive(int listenpin) {
int average = 12;
int listen[average];
int index1;
int highindex = 0;
int high = 0;
int lowindex = 0;
int low = 1000;
int total = 0;

for (index1 = 0; index1 < average; index1 = index1 + 1) {
listen[index1]= analogRead(listenpin);
// Serial.println(listen[index1]);
}

for (index1 = 0; index1 < average; index1 = index1 + 1) {
if (listen[index1] > high) {
high = listen[index1];
highindex = index1;
}
}

for (index1 = 0; index1 < average; index1 = index1 + 1) {
if (listen[index1] < low) {
low = listen[index1];
lowindex = index1;
}
}

for (index1 = 0; index1 < average; index1 = index1 + 1) {
if ((index1 != highindex) && ( index1 != lowindex)) {
total = total + listen[index1];
}
}

total = int(total / (average-2));
//Serial.print ("average: ");
//Serial.println(total);
return total;
}




void Listen() {
Serial.println("====== Listen ======!");
for (deg = 0; deg < 71; deg = deg + 1) {
// soundRT[deg]= Averagefive(RightEarPin); // listen to right ear and assign value to array
// soundLF[deg]= Averagefive(LeftEarPin); // listen to left ear and assign value to array
distance[deg] = Averagefive(SonarPin); // read a value from the sensor
Serial.println(distance[deg]); // display value written to distance array for this direction
Right();
mydelay(5);
Stop();
}
Stop();
}

void Course() {
int BestDistDeg = 0;
int BestDist = 0 ;
for (deg = 0; deg < 71; deg = deg + 1) {
// soundRT[deg]= analogRead(RightEarPin); // listen to right ear and assign value to array
// soundLF[deg]= analogRead(LeftEarPin); // listen to left ear and assign value to array
if (distance[deg+1] > distance[BestDistDeg]) {
BestDistDeg = deg + 1;
}
Serial.print("BestDist=");
Serial.println(distance[BestDistDeg]);
Serial.println(BestDistDeg); // display value written to distance array for this direction
}
Left();
mydelay((71-BestDistDeg) * 5); //Rotate Right until course matching longest distance is attained.
Stop();
Forew();
}

void ForewardBackward() {
Serial.println("*** Foreward Backward test Start!");
Forew();
delay(2000);
Stop();
delay(2000);
Backw();
delay(2000);
Stop();
delay(2000);
Serial.println("*** Foreward Backward test End!");
}

void base(){
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();
delay(2500);
myservoFR.attach(FRpin);
myservoFL.attach(FLpin);
myservoRR.attach(RRpin);
myservoRL.attach(RLpin);
Serial.println('sending 110');
myservoFR.write(180);
myservoFL.write(0);
myservoRR.write(180);
myservoRL.write(0);
delay(2500);
myservoFR.detach();
myservoFL.detach();
myservoRR.detach();
myservoRL.detach();
delay(2500);
myservoFR.attach(FRpin);
myservoFL.attach(FLpin);
myservoRR.attach(RRpin);
myservoRL.attach(RLpin);
Serial.println('sending70');
myservoFR.write(0);
myservoFL.write(180);
myservoRR.write(0);
myservoRL.write(180);
delay(2500);
}

void degreetest() {
Serial.println("degreetest start");
Stop();
delay(5000);
Right();
mydelay(360);
Serial.println("degreetest end");
}

void Signalblink() {
LEDWhite();
delay(500);
LEDOff();
delay(500);
}

void forwardinfertest() {
delay(500);
Serial.print("forward infered senor: ");
Serial.println(analogRead(InferFPin));
//danger threshold <200
}


void backwardinfertest() {
delay(500);
Serial.print("backward infered senor: ");
Serial.println(analogRead(InferBPin));
//danger threshold <300
}

void sonartest() {
delay(500);
Serial.print("Sonar senor: ");
Serial.println(analogRead(SonarPin));
//danger threshold <30
}

int VoltageCheck(){

int index1 =0;
double voltsum = 0;
int analogvolt = 0;

for (index1 = 0; index1 < VoltSampleSize; index1 = index1 + 1) {
voltsum = voltsum + analogRead(VoltPin);
}
analogvolt = int (voltsum / VoltSampleSize);
//Serial.print("VoltageCheck: ");
//Serial.print("analag volt: ");
//Serial.println(analogvolt);
//Serial.print("actual volt: ");
//Serial.println((analogvolt / analogvoltconstant));
//Serial.println();
return analogvolt;
}

void BatteryCheckLED()
{
if (VoltageCheck() > shutdownVoltageCheck) {
digitalWrite(LEDRed, LOW);
digitalWrite(LEDGreen, HIGH);
} else {
digitalWrite(LEDRed, HIGH);
digitalWrite(LEDGreen, LOW);
}
BatteryIndex = 0;
}

void RegulatorControlToggle()
{
power = power * -1;
if (power == 1) {
digitalWrite(regulator, HIGH);
}
if (power == -1) {
digitalWrite(regulator, LOW);
}
RegulatorIndex = 0;
}

void Miser()
{
RegulatorIndex = RegulatorIndex + 1;
if (RegulatorIndex == RegulatorTimerThreshold) RegulatorControlToggle();
}

//********************************************* end mine *******************************************



void performCommand(char* com) {
float volt=0;

if (strcmp(com, "f") == 0 ) { // Forward
Forew();
} else if (strcmp(com, "r") == 0) { // Right
Right();
} else if (strcmp(com, "l") == 0) { // Left
Left();
} else if (strcmp(com, "b") == 0) { // Backward
Backw();
} else if (strcmp(com, "s") == 0) { // Stop
Stop();
} else if (strcmp(com, "fr") == 0 || strcmp(com, "fz") == 0 || strcmp(com, "x") == 0) {     // print forward facing distance sensor
dist = Averagefive(SonarPin);
itoa(dist, msg, 10); // Turn the dist int into a char
serialReply("x", msg); // Send the distance out the serial line
} else if (strcmp(com, "z") == 0) { // Read and print ground facing distance sensor
dist = Averagefive(SonarPin); // dist = getDistanceSensor(rangePinForwardGround);
itoa(dist, msg, 10); // Turn the dist int into a char
serialReply("z", msg); // Send the distance out the serial line
Serial.println(dist);
} else if (strcmp(com, "v") == 0){ // Read and print VoltageCheck
volt = VoltageCheck();
itoa(volt, msg, 10); // Turn the volt int into a char
Serial.println(volt);
serialReply("v", msg); // Send the distance out the serial line
} else if (strcmp(com, "h") == 0) { // Help mode - debugging toggle
//Signalblink();
} else if (com[0] == 'n') { // Move head up
//sscanf (com,"n %d",&lastNeckValue); // Parse the input into multiple values
//myservoHead.attach(servoPinHead);
// myservoHead.write(lastNeckValue);
//EEPROM.write(EEPROM_lastNeckValue, lastNeckValue);
// if (DEBUGGING) {
// Serial.print("Neck moved to ");
// Serial.println(lastNeckValue);
//}
} else if (com[0] == 'p') { // Initiates Bluetooth pairing so another device can connect
pairBluetooth();
} else {
serialReply("e", com);// Echo unknown command back
// if (DEBUGGING) {
// Serial.print("Unknown command: ");
// Serial.println(com);
// }

// 0-9, w, n, w, i, c, h, d for demo,
}
}



// =========================== Main loop running at all RegulatorIndexs =====================================
void loop()
{
readSerialInput();
checkIfStopBot();

BatteryIndex = BatteryIndex + 1;
if (BatteryIndex == BatteryIndexThreshold) BatteryCheckLED();

if (ConservePower) Miser();
}