Hacking a Rumble Robot

Hi. This is my version of utilizing an Arduino Uno to hack a Rumble Robot, a toy circa 2000 and 2001. Credit for the start of this project is at:
http://www.dinofab.com/rumblebot.html

The goal is to create a seeker robot. I put two ping sensors on this. Left will ping, then right, then left, then right. Those values are stored in an array. This gives 4 values to utilize for analysis. The goal is to detect movement. I found that there is some variation in the ping measurements and also, the sensors are not exactly aligned, so there is a value for tolerance. If there is a change in value then movement occurred and the robot will move towards the movement.

Here is the sketch. I have so that it will circle and seek. But I don't have a robot for it to chase, that will come next week. Sorry the code is not super clean. But that is a work in progress.

++++++++++++++++++++++++++++++++++++++++

//This is to test how to make functions work. Good.
//LEDs blink just fine.
//Two pingers ping and return the distance in cm.
//This has a memory creation array.
//Now to use the distance measuring and have decisions based on that.

int vcount = 0;
int vSonarTime = 0; //This will the value of the time of the sonar ping, which is the distance
int arrSonarPin[] = {
4,2}; //this is left then right sonar
int svar = 0; // variable for something
int sonarLeft = 0;
int sonarRight = 0;
int vTolerate = 10; //this is the microseconds of error in the ping that gets ignored
int arrScanz[96]; // this is where decisions go about comparePing decisions
int arrScanzEnd = 95; // end element number for array arrScanz
int vScanz = 0; //global var accessed to record results of scan analysis
int vMovementRight = 3; // initialize with 3 to see if there is an error later
int vMovementLeft = 3; // initialize with 3 to see if there is an error later
int vCloserRight = 3;
int vCloserLeft = 3;
int vStayPut = 3;

int aPoints[48]; // want to make array points and distances
int PointsEnd = 47;
int vPoint = 0; // This is init'd for use as the increasing array elements where distances get stored.
// Because this is global we can usitilize it in functions. But only StorePoint() will modify vPoint
int i ; // for counting in for loops
int LedA = 13; // onboard
int LedB = 12; //red
int LedC = 9; //red
int LedD = 8; //green
int greenLED = 8; //easier to recall

int arrLed[] = {LedA, LedB, LedC, LedD};
int arrLedEnd = 3; // the array arrLed has 4 members, so last is #3

int motorpinright = 10; // pin for right motor red
int motorpinleft = 11; // pin for left motor forward yellow
int motorpinrevright = 5; // pin for right motor reverse black
int motorpinrevleft = 6; // pin for right motor forward green
int vMoveTime = 200; // this is var for the time moved
int vDelayTime = 250;

void setup() {
Serial.begin(9600);
pinMode(LedA,OUTPUT);
pinMode(LedB,OUTPUT);
pinMode(LedC,OUTPUT);
pinMode(LedD,OUTPUT);
pinMode(motorpinright, OUTPUT); // Motor drives-----------
pinMode(motorpinleft, OUTPUT); //------------------------
pinMode(motorpinrevright, OUTPUT); //------------------------
pinMode(motorpinrevleft, OUTPUT); //------------------------
for (i = 0; i < PointsEnd; i++){ // need to initialize the distances aPoints[] array
aPoints[i] = 0;
} // End the for loop
for (i = 0; i < arrScanzEnd; i++){ // need to initialize the arrScanz[] array
arrScanz[i] = 0;
} // End the for loop

} // end void loop

void loop() {
// digitalWrite(13,LOW); // set low to start
Serial.println(" Start into main loop");

blinxAll(arrLedEnd); //Call fancy Ass function. Do not assign it an external value

ShowArrayContents();

BlinxOne(greenLED,100,6);

DetectMovement(); // Does pings in 3 directions

DecideWhichWay(); // if no movement detected then move and seek
// end the decision to move

// vMoveTime = 200;
// Serial.print(" Just out of sonarx. vSonarTime = ");
// Serial.println(vSonarTime);
// Serial.println(" Out of function blinxAll");
// arrLedEnd = 3;

int vMovementRight = 3; // initialize with 3 to see if there is an error later
int vMovementLeft = 3; // initialize with 3 to see if there is an error later
int vStayPut = 3;

} // end the loop
//+++++++++++++++++++++++++++++++++++++++++++
// ***** Declare Functions *****
//+++++++++++++++++++++++++++++++++++++++++++
int sonarx(int vpin){ // this function will call the sonar module to ping and return a value of the ping
int x = 5;
// int vPingDist = 0; // no onger needed
int vDuration; //, inches, cm;
x = vpin; // this is the pin number
Serial.print( " in sonarx and value vpin is : ");
Serial.print(x);
pinMode(vpin, OUTPUT);
digitalWrite(vpin, LOW);
delayMicroseconds(2);
digitalWrite(vpin, HIGH);
delayMicroseconds(5);
digitalWrite(vpin, LOW);

// The same pin is used to read the signal from the PING))): 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(vpin, INPUT);
vDuration = pulseIn(vpin, HIGH);

// cm = microsecondsToCentimeters(vDuration);

Serial.print(" vDuration: ");
Serial.println(vDuration);

return vDuration;
}
//+++++++++++++++++++++++++++++++++++++++++++

int microsecondsToCentimeters(int 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;
}

//+++++++++++++++++++++++++++++++++++++++++++
void blinxAll (int arrLedEndi){
int ivar1;

Serial.print("In the blinxAll function, value is :");
Serial.println(arrLedEndi);
delay(100);


for (int i=0; i <= arrLedEndi ; i++){
ivar1 = arrLed[i]; /* i is the place in the array, and this changes each loop.
The value of the array is assigned. This is the pin number of the LED
that will be outputted.*/
// Serial.print(" the value of ivar1 is: ");
// Serial.print(ivar1);
digitalWrite(ivar1,HIGH);
delay(100);
digitalWrite(ivar1,LOW);
delay(100);
} // End the For cycle through array

Serial.println(" End of function blinxAll");

} // End function
//+++++++++++++++++++++++++++++++++++++++++++
void BlinxOne (int vLED, int vDelay, int numBlinks){ // vLED has choices from above. vDelay is
// int vLED;
vDelay = 100;
Serial.print("In the BlinxOne function, value is :");
Serial.println(vLED);
delay(vDelay);
for (int i=0; i <= numBlinks ; i++){
digitalWrite(vLED,HIGH);
delay(vDelay);
digitalWrite(vLED,LOW);
delay(vDelay);
} //End of for loop
Serial.println(" End of function BlinxOne");

} // End function BlinxOne
//+++++++++++++++++++++++++++++++++++++++++++
void MoveAhead(){
int ivar1 = LedD;
digitalWrite(ivar1,HIGH);

int v2MoveTime = vMoveTime;
Serial.print("vMoveTime MoveAhead is worth : ");
Serial.println(v2MoveTime);
// This will move bot ahead for a time passed into function
//analogWrite(motorpinleft, 0); //stop left motor
//analogWrite(motorpinright, 0); //stop right motor
analogWrite(motorpinleft, 255); //stop left motor
analogWrite(motorpinright, 255); //stop right motor
delay(vMoveTime);
analogWrite(motorpinleft, 0); //stop left motor
analogWrite(motorpinright, 0); //stop right motor

digitalWrite(ivar1,LOW); // turn off green light
vMoveTime = 0; // prevent endless loops ++ runaways that is
}
//+++++++++++++++++++++++++++++++++++++++++++
void MoveBack(){
Serial.println("Moving Back OK !!");
int v2MoveTime = vMoveTime;
Serial.print("vMoveTime is worth : ");
Serial.println(v2MoveTime);

analogWrite(motorpinrevleft, 255); //stop left motor
analogWrite(motorpinrevright, 255); //stop right motor
delay(vMoveTime);
analogWrite(motorpinrevleft, 0); //stop left motor
analogWrite(motorpinrevright, 0); //stop right motor
vMoveTime = 0; // prevent endless loops ++ runaways that is
}
//+++++++++++++++++++++++++++++++++++++++++++
void TurnRight90(){
int v2MoveTime = vMoveTime;
Serial.print("vMoveTime TurnRight90 is worth : ");
Serial.println(v2MoveTime);

analogWrite(motorpinrevleft, 255); //stop left motor
analogWrite(motorpinright, 255); //stop right motor
delay(vMoveTime);
analogWrite(motorpinrevleft, 0); //stop left motor
analogWrite(motorpinright, 0); //stop right motor
vMoveTime = 0; // prevent endless loops ++ runaways that is
}
//+++++++++++++++++++++++++++++++++++++++++++

void TurnLeft45(){
int vMoveTime2 = (vMoveTime/2);
Serial.print("vMoveTime TurnLeft45 is worth : ");
Serial.println(vMoveTime2);

analogWrite(motorpinleft, 0); //start left motor
// delay(vMoveTime2); // This is half to make the turn
analogWrite(motorpinright, 255); //start right motor
delay(vMoveTime);
// analogWrite(motorpinleft, 0); //stop left motor
analogWrite(motorpinright, 0); //stop right motor
vMoveTime = 0; // prevent endless loops ++ runaways that is
}
//+++++++++++++++++++++++++++++++++++++++++++

void TurnRight45(){
int vMoveTime2 = (vMoveTime/2);
Serial.print("vMoveTime TurnRight45 is worth : ");
Serial.println(vMoveTime2);

analogWrite(motorpinleft, 255); //start left motor
delay(vMoveTime);
analogWrite(motorpinright, 0); //start right motor
// delay(vMoveTime2); // This is half to make the turn
analogWrite(motorpinleft, 0); //stop left motor
// analogWrite(motorpinright, 0); //stop right motor
vMoveTime = 0; // prevent endless loops ++ runaways that is
}

//+++++++++++++++++++++++++++++++++++++++++++
void TurnLeft90(){
int v2MoveTime = vMoveTime;
Serial.print("vMoveTime TurnLeft90 is worth : ");
Serial.println(v2MoveTime);

analogWrite(motorpinleft, 255); //stop left motor
analogWrite(motorpinrevright, 255); //stop right motor
delay(vMoveTime);
analogWrite(motorpinleft, 0); //stop left motor
analogWrite(motorpinrevright, 0); //stop right motor
vMoveTime = 0; // prevent endless loops ++ runaways that is
}
//+++++++++++++++++++++++++++++++++++++++++++
void AllStop(){
analogWrite(motorpinleft, 0); //stop left motor
analogWrite(motorpinright, 0); //stop right motor
analogWrite(motorpinrevright, 0); // stop right rev motor
analogWrite(motorpinrevleft, 0); // stop left rev motor

}
//+++++++++++++++++++++++++++++++++++++++++++
// function StorePoint will store point values in an array
void StorePoint (){
if (vPoint <= PointsEnd) {
aPoints[vPoint] = sonarx(svar); // make this a function sometime
vPoint ++ ; // increment vPoint variable
}
else {
vPoint = 0;
aPoints[vPoint] = sonarx(svar); // make this a function sometime
}
}
//+++++++++++++++++++++++++++++++++++++++++++
// function StoreScanz
void StoreScanz (int vfScan){
if (vScanz <= arrScanzEnd){
arrScanz[vScanz] = vfScan;
vScanz ++ ; // increment vScanz the counter
} else
{
vScanz = 0;
arrScanz[vScanz] = vfScan;
}
}
//+++++++++++++++++++++++++++++++++++++++++++
void get4pings() {
// Get 2 sets of left and right values.
for (int f = 0; f <= 1; f++){
// Do the ping function, first left then right.
for(int b =0; b <=1; b++) {
svar = arrSonarPin[b]; // choose the left then right ping pin: 4 is left now
//vSonarTime = sonarx(svar); //Call function sonarx to measure the distance time of the ping.
// assigning this value to vSonarTime is likely useless
StorePoint(); //sonarx function is called in this function vPoint is a Global Var

// the current distance is stored in an array element
if(b==0){
Serial.print(" sonarx assigned Left Hand vPoint value = ");
Serial.println(vPoint); // vPoint is the global Variable to use.
}
else {
Serial.print(" sonarx assigned Right Hand vPoint value = ");
Serial.println(vPoint); // vPoint is the global Variable to use.
}// end the if else
} // end of for loop. Two values assigned into memory array. 2 sets of left and right values.
} // End the for loop that gets 4 values
} //End function get4 values
//++++++++++++++++++++++++++++++++++++++++++++
// A function to print out the array contents.

void ShowArrayContents() {
//this is used for diagnostics while trouble shooting.
for (i = 0; i <= PointsEnd ; i ++)// print out the array of measured distances
{
Serial.print("i = ");
Serial.print(i);
Serial.print(" aPoints Value is = ");
Serial.print(aPoints[i]);
Serial.println();
}// End the for loop.
} // End function ShowArrayContents
//+++++++++++++++++++++++++++++++++++++++++++++
int comparePings() // if we know the position of the last array element writen, then we can compare thingz
{ // start of function
int s = 1; // this will only go through once.
int t = 2;
int vComparePingsLD = abs((aPoints[vPoint-1] )- (aPoints[vPoint-3])); // this shows 'D'epth movement
int vComparePingsRD = abs((aPoints[vPoint-0] )- (aPoints[vPoint-2]));
int vComparePingsRC = abs((aPoints[vPoint-0] )- (aPoints[vPoint-1])); //this shows which side has 'C'losest ping
int vComparePingsLC = abs((aPoints[vPoint-3] )- (aPoints[vPoint-4]));

Serial.print (" vComparePingsLD value is : ");
Serial.println (vComparePingsLD);
Serial.print (" vComparePingsRD value is : ");
Serial.println (vComparePingsRD);
Serial.print (" vComparePingsRC value is : ");
Serial.println (vComparePingsRC);
Serial.print (" vComparePingsLC value is : ");
Serial.println (vComparePingsLC);

while (s < t) { // the resaon to look at there is a while loop only worked once.
if ((aPoints[vPoint-1] == aPoints[vPoint-3]) || (vComparePingsLD <= vTolerate))
{ //if these are eqivalent then there is no motion towards or away from pinger
arrScanz[vScanz] = 0; //no movement
Serial.print (" The left side depth points are equivalent: ") ;
Serial.print (aPoints[vPoint-3]);
Serial.print (" ");
Serial.println (aPoints[vPoint-1]);
vScanz++;
// vScanz++;
}
else {
vMovementLeft = 1;
arrScanz[vScanz] = 1; // there was movement
vScanz++;
// vScanz++;
}

if (((aPoints[vPoint]) == (aPoints[vPoint-2])) || (vComparePingsRD <= vTolerate))
{ //if these are eqivalent then there is no motion towards or away from pinger
arrScanz[vScanz] = 0; //no movement
Serial.print (" The right side depth points are equivalent: ") ;
Serial.print (aPoints[vPoint-2]);
Serial.print (" ");
Serial.println (aPoints[vPoint]);
vScanz++;
}
else {
vMovementRight = 1;
arrScanz[vScanz] = 1; // there was movement
vScanz++;
// vScanz++;
}

if (((aPoints[vPoint]) == (aPoints[vPoint-1])) || (vComparePingsRC <= vTolerate))
{ //check to see which way has closer object
Serial.print (" The newest two distances of left and right are left: ") ;
Serial.print (aPoints[vPoint-1]);
Serial.print (" then right: ");
Serial.println (aPoints[vPoint]);
vScanz++;
}
else {
if(( aPoints[vPoint - 1]) < (aPoints[vPoint])) { // start if
vCloserLeft = 1;
vCloserRight = 0;
} // end if
else
{
vCloserLeft = 0;
vCloserRight = 1;
// arrScanz[vScanz] = 1; // there was movement
vScanz++;
// vScanz++;
} // end else
} // end else

if (((aPoints[vPoint-3]) == (aPoints[vPoint-4])) || (vComparePingsLC <= vTolerate))
{ //check to see which way has closer object
//Don't think I need this currently. hmmmm
Serial.print (" The older two distances of left and right are left: ") ;
Serial.print (aPoints[vPoint-4]);
Serial.print (" then right: ");
Serial.println (aPoints[vPoint-3]);
vScanz++;
}
else
{
arrScanz[vScanz] = 1; // there was movement
vScanz++;
}
// end if
s++; // increment for the while loop
} //end the while function
delay(500); // To let me read the serial Monitor. Remove this later.
} // end of function
//++++++++++++++++++++++++++++++++++++++++++++
void DetectMovement(){ //Does pings in 3 direcctions
//go left45 ping then go right45 (which is center) then right45 then decide if there is movement.
int s = 0;
int t = 2; // this can be adjusted for more scans
int fDelay = 100; // internal number for the delay between scans
// vMoveTime = 500; // This is a global variable that is changed a lot
// TurnLeft45(); //This is the initial arbitary move. Will make sorting the scan data easier.
Serial.println("Turn left45, get values");

while (s < t) {
get4pings();
delay(fDelay);
comparePings(); // when this is called then the comparisons will decide movement or no movement

s = s++;
// TurnRight45();
Serial.println("Turn right45, get values");
} // end while
s = 0; //reset the function variable

// Compare the collected values.

delay(fDelay);

} // End DetectMovement function
//++++++++++++++++++++++++++++++++++++++++++++
void DecideWhichWay(){

if (aPoints[vPoint] > 200) {
vMoveTime = (aPoints[vPoint]/10); // gives a scale to use based on distance
}
else { // too close so turn only
vMovementLeft = 0;
vMovementRight = 0;
}

if (vMovementLeft == 1) {
TurnLeft45 ();
MoveAhead ();
} // end if
else if (vMovementRight == 1) {
TurnRight45 ();
MoveAhead ();
} //
else if (vStayPut == 0) {
vMoveTime = vMoveTime;
MoveAhead();
} //

else { // stay in place, but rotate right 90 to scan new area.
TurnRight90();
AllStop();
} // end else

} // End function DecideWhichWay
//+++++++++++++++++++++++++++++++++++++++++++++