Hack-A-Toy Fire Seeking Sumo Robot

Front view of the sumo…
Back view of the sumo…

Bottom view of the sumo, notice the two bigger outside wheels. Those wheels are driven by the mechanism we’ll see in a minute.  Also notice the the center section containing all of the electronics actually unscrews and comes out of the softer sumo body. This made it easy to get inside and hack what I needed…

Here is the front view with the electronics removed…

Back view of the sumo with the electronics removed and labeled.

To get to the internal electronics I had to remove the battery cover and batteries with one screw shown above under the big R. and then 4 screws on the outside in order to get to the inside electronics that look like this…

What I very quickly found out was that the motors and their electronics were easily accessible and pretty much ready for me to hack.  You can see below that the leads to each motor were exposed and already setup with capacitors.  All I needed to do was remove the existing wires and connect my own. Red/Black motor on this side.

The white/yellow motor on the other side.

Here is a view of the bottom side as well where you can see the big drive wheels…

[amazon_link asins=’B079189ZNV’ template=’ProductGrid’ store=’scottkerfootcom-20′ marketplace=’US’ link_id=’bc152951-9a98-11e8-9c7f-b96d7b110712′]

Once I figured out how it worked I decided to create a fire seeking sumo robot. I would use the sensor from lab 14 and control the motors to find the heat. The logic behind this, which I built into the code, was basically to start from where you are now, take an IR reading, move left a little, take another reading, if left was bigger stay left and restart the loop.  If left was smaller or equal I would move right two steps (one to get to center, the other to go right 1) and check the right reading. If right was bigger by 5 I would move right and restart the loop. If center was bigger I would move back left 1, and then move forward and restart the loop. Also at the beginning of the loop I would check to see if my sensor was already “close to fire” (value > 900 in my case), if it was I would stop the loop and blink the two LED’s I had wired into the circuit for 10 seconds and then start the process again.

In order to implement it and make it look right I drilled some small holes in the top of the electronics case and a hold in the sumo’s belly button for the IR sensor (cool don’t you think!) and in the middle of his back for all the wires used to control the circuits. I used two h-bridge circuits to control the individual motors and then did all the implementation in code.  In hindsight I should have built a better wiring harness because the wires ended up weighing more than the robot making movement hard which you’ll see in the video.

The code also uses the two LED’s to indicate moving Right, Left, Stopped, or Found Fire

Here is the sumo with the circuit plugged in as well as the breadboarded circuitry.

Front view, note the IR sensor “hidden” in the belly button.

Rear view with wires going into the body….

Here is the top view where you can see all of the circuitry. Notice the two h-bridges as well as the diodes that will blink when fire is found or show left or right based on the direction the robot is moving.

Finally, here is the video of the Fire Seeking Sumo Robot trying it best to fight against the oppressive wiring harness to see out fire.

With some more time I would have mounted the LED’s in the eyes and made a longer better wiring harness so it would move better. If nothing else I will be able to re-use almost all of my code for the VEX. 🙂  I started coding by writing some test harnesses (not much commenting on these because they aren’t the production code).

First test harness is for the motor and movement.

#define pin1 5
#define pin2 6
#define pin3 9
#define pin4 10

void setup() {
  // put your setup code here, to run once:
  pinMode(pin1,OUTPUT);
  pinMode(pin2,OUTPUT);
  pinMode(pin3,OUTPUT);
  pinMode(pin4,OUTPUT);
  Serial.begin(9600);
}

void loop() {
  // put your main code here, to run repeatedly:
  Serial.print(“Forward “);
  digitalWrite(pin1,HIGH);
  digitalWrite(pin2,LOW);
  digitalWrite(pin3,HIGH);
  digitalWrite(pin4,LOW);
  delay(3000);
  Serial.print(“Backward “);
  digitalWrite(pin1,LOW);
  digitalWrite(pin2,HIGH);
  digitalWrite(pin3,LOW);
  digitalWrite(pin4,HIGH);
  delay(3000);
  Serial.println(“Stop!”);
  digitalWrite(pin1,LOW);
  digitalWrite(pin2,LOW);
  digitalWrite(pin3,LOW);
  digitalWrite(pin4,LOW);
  
  delay(3000);

}

The next test harness was for the LED eyes.

// Define our eye information
#define EYEPIN1 2
#define EYEPIN2 3
#define EYESOFF 0
#define EYESBLINK 1
#define EYESLEFT 2
#define EYESRIGHT 3
#define EYESON 9
#define EYEBLINKDUR 100

#define TESTDELAY 2000

int counter = 0;

void setup() {
  // put your setup code here, to run once:
  pinMode(EYEPIN1,OUTPUT);
  pinMode(EYEPIN2,OUTPUT);

  // Now initialize our debugging.
  Serial.begin(9600);
  eyes(EYESOFF);
}

void eyes(int eyestat) {
  if (eyestat == EYESOFF) {
    digitalWrite(EYEPIN1,LOW);
    digitalWrite(EYEPIN2,LOW);
  } else if (eyestat == EYESBLINK) {
    // Blink the eyes 10 time.
    for (counter=1;counter<30;counter++) {
      digitalWrite(EYEPIN1,HIGH);
      digitalWrite(EYEPIN2,LOW);
      delay(EYEBLINKDUR);
      digitalWrite(EYEPIN1,LOW);
      digitalWrite(EYEPIN2,HIGH);
      delay(EYEBLINKDUR);
    }
    eyes(EYESON);
  } else if (eyestat == EYESLEFT) {
    // Look left
    digitalWrite(EYEPIN1,HIGH);
    digitalWrite(EYEPIN2,LOW);
  } else if (eyestat == EYESRIGHT) {
    // Look right
    digitalWrite(EYEPIN1,LOW);
    digitalWrite(EYEPIN2,HIGH);
  } else if (eyestat == EYESON) {
    // Turn the eyes on.
    digitalWrite(EYEPIN1,HIGH);
    digitalWrite(EYEPIN2,HIGH);
  }
}
void loop() {
  // put your main code here, to run repeatedly:
  eyes(EYESON);
  delay(TESTDELAY);
  eyes(EYESBLINK);
  delay(TESTDELAY);
  eyes(EYESOFF);
  delay(TESTDELAY);
  eyes(EYESLEFT);
  delay(TESTDELAY);
  eyes(EYESRIGHT);
  delay(TESTDELAY);
  eyes(EYESOFF);
  delay(TESTDELAY);
  
}

And then finally this is where I bring the test harnesses and the sensor code from lab 14 together into the full sumo logic. Here is the code I wrote in two days.

/*
   program: hackatoylsk
   date: 1/31/2017
   author: Leslie Scott Kerfoot
   This program will automate my hack a toy.
*/

// Define the on/off switch
#define SWITCHPIN 12

// Define the heat sensor
#define HEATSENSOR A0
#define HEATFOUND 800

// Define the motors
#define LEFTMOTOR 1
#define RIGHTMOTOR 2

// Define our motor pins
#define LEFTMOTORPIN1 5
#define LEFTMOTORPIN2 6
#define RIGHTMOTORPIN1 9
#define RIGHTMOTORPIN2 10

// Define our motor states
#define MOTORSTOP 0
#define MOTORFORWARD 1
#define MOTORBACKWARD 2

// Define our toy movement
#define MOVESTOP 0
#define MOVEFORWARD 1
#define MOVEDUR 50

// Define our eye information
#define EYEPIN1 2
#define EYEPIN2 3
#define EYESOFF 0
#define EYESBLINK 1
#define EYESLEFT 2
#define EYESRIGHT 3
#define EYESON 9
#define EYEBLINKDUR 100


// Define what we’ll need to track state.
int sensorvalue = 0;
int sensorleft = 0;
int sensorright = 0;
int sensormax = 0;
int sensormin = 1024;
int sensorlast = 0;
int foundheat = 0;
int mintomove = 0; // This is the minimum sensor value we’ll accept for a move.

// Some misc variables we’ll need so they don’t create wasted space.
int switchstate = 0;
int counter = 0;

// Setup our environment
void setup() {
  // put your setup code here, to run once:
  pinMode(LEFTMOTORPIN1, OUTPUT);
  pinMode(LEFTMOTORPIN2, OUTPUT);
  pinMode(RIGHTMOTORPIN1, OUTPUT);
  pinMode(RIGHTMOTORPIN2, OUTPUT);
  pinMode(HEATSENSOR, INPUT);
  pinMode(EYEPIN1, OUTPUT);
  pinMode(EYEPIN2, OUTPUT);
  pinMode(SWITCHPIN, INPUT);

  // Now initialize our debugging.
  Serial.begin(9600);
  // Stop the device if it’s moving.
  movestop();
  eyes(EYESOFF);
}

/*
   function: motorgo
   date: 1/21/2017
   author: leslie scott kerfoot @scottker
   description: this will move a particular motor base on the
*/
int motorgo(int whichmotor, int motorstate, int motorspeed) {
  // Setup the variable names we’ll need for our motor.
  int motorpin1 = 1;
  int motorpin2 = 2;

  // Error check our motors.
  if ((whichmotor < 1) || (whichmotor > 2)) {
    Serial.print(” *BM*”);
    whichmotor = LEFTMOTOR;
  }

  // Error check our motor state
  if (!((motorstate == MOTORSTOP) || (motorstate == MOTORFORWARD) || (motorstate == MOTORBACKWARD)))  {
    Serial.print(” *BS*”);
    motorstate = MOTORSTOP;
  }

  // Error check the motorspeed.
  if ((motorspeed < LOW) || (motorspeed > HIGH)) {
    Serial.print(” *BP*”);
    motorspeed = LOW;
  }

  // Assign our motor pins depending on the motor we passed.
  if (whichmotor == LEFTMOTOR) {
    motorpin1 = LEFTMOTORPIN1;
    motorpin2 = LEFTMOTORPIN2;
  } else {
    motorpin1 = RIGHTMOTORPIN1;
    motorpin2 = RIGHTMOTORPIN2;
  }

  if (motorstate == MOTORSTOP) {
    Serial.print(“0”);
    digitalWrite(motorpin1, LOW);
    digitalWrite(motorpin2, LOW);
  }

  if (motorstate == MOTORFORWARD) {
    Serial.print(“F”);
    digitalWrite(motorpin1, HIGH);
    digitalWrite(motorpin2, LOW);
  }

  if (motorstate == MOTORBACKWARD) {
    Serial.print(“B”);
    digitalWrite(motorpin1, LOW);
    digitalWrite(motorpin2, HIGH);
  }
}

void moveforward(int duration) {
  Serial.print(” MF”);
  if (duration > 0) {
    motorgo(LEFTMOTOR, MOTORFORWARD, LOW);
    motorgo(RIGHTMOTOR, MOTORFORWARD, LOW);
    eyes(EYESON);
    delay(duration*2);
  }
  movestop();
}

void moveright(int duration) {
  Serial.print(” MR”);
  Serial.print(duration);
  if (duration > 0) {
    eyes(EYESRIGHT);
    motorgo(LEFTMOTOR, MOTORBACKWARD, HIGH);
    motorgo(RIGHTMOTOR, MOTORFORWARD, HIGH);
    delay(duration);
  }
  movestop();
//  moveback(MOVEDUR);
}

void moveleft(int duration) {
  Serial.print(” ML”);
  if (duration > 0) {
    eyes(EYESLEFT);
    motorgo(LEFTMOTOR, MOTORFORWARD , HIGH);
    motorgo(RIGHTMOTOR,MOTORBACKWARD , HIGH);
    delay(duration);
  }
    movestop();
//    moveback(MOVEDUR);
}

void moveback(int duration) {
  Serial.print(” MB”);
  if (duration > 0) {
    motorgo(LEFTMOTOR, MOTORBACKWARD, HIGH);
    motorgo(RIGHTMOTOR, MOTORBACKWARD, HIGH);
    eyes(EYESOFF);
    delay(duration);
  }
  movestop();
}

void movestop() {
  Serial.print(” MS!”);
  motorgo(LEFTMOTOR, MOTORSTOP, LOW);
  motorgo(RIGHTMOTOR, MOTORSTOP, LOW);
  delay(MOVEDUR*5);
}

void eyes(int eyestat) {
  if (eyestat == EYESOFF) {
    digitalWrite(EYEPIN1, LOW);
    digitalWrite(EYEPIN2, LOW);
  } else if (eyestat == EYESBLINK) {
    // Blink the eyes 10 time.
    for (counter = 1; counter < 30; counter++) {
      digitalWrite(EYEPIN1, HIGH);
      digitalWrite(EYEPIN2, LOW);
      delay(EYEBLINKDUR);
      digitalWrite(EYEPIN1, LOW);
      digitalWrite(EYEPIN2, HIGH);
      delay(EYEBLINKDUR);
    }
    eyes(EYESON);
  } else if (eyestat == EYESLEFT) {
    // Look left
    digitalWrite(EYEPIN1, HIGH);
    digitalWrite(EYEPIN2, LOW);
  } else if (eyestat == EYESRIGHT) {
    // Look right
    digitalWrite(EYEPIN1, LOW);
    digitalWrite(EYEPIN2, HIGH);
  } else if (eyestat == EYESON) {
    // Turn the eyes on.
    digitalWrite(EYEPIN1, HIGH);
    digitalWrite(EYEPIN2, HIGH);
  }
}

void loop() {
  // For debugging we’ll do one line per loop…
  Serial.print(“Go”);

  // Read the sensor value.
  sensorvalue = analogRead(HEATSENSOR);
  mintomove = sensorvalue + 10;
  Serial.print(” “);
  Serial.print(sensorvalue);

  switchstate = digitalRead(SWITCHPIN); // Is the switch pushed.

  if (sensorvalue > HEATFOUND) {
    // We found our heat, we’re close enough, blink the lights.
    Serial.print(” Found Heat!!!”);
    eyes(EYESBLINK);
  } else if (switchstate == HIGH) {
    // Is it turned on or off.
    // Process only if the switch is on

    // We need to look left and look right
    Serial.print(” Seek”);

    // Look to the left.
    moveleft(MOVEDUR);
    sensorleft = analogRead(HEATSENSOR);
    Serial.print(” L”);
    Serial.print(sensorleft);

    // Did we find more heat to the left?
    if (sensorleft >= mintomove) {
      // sensorleft >= mintomove
      //   looks like left is where it’s at, just exit the loop and try again.
      Serial.print(” Go Left”);
    } else {
      // We didn’t find a reason to turn left so turn right past center.
      moveright(MOVEDUR * 2); // Move past center to the right.
      sensorright = analogRead(HEATSENSOR); // Read the sensor to the right.
      Serial.print(” R”);
      Serial.print(sensorright);
      // Check to see if right is better than left
      if (sensorright >= mintomove || sensorvalue < 5) {
        // Looks like the value to the left is better than straight.
        // Stay here and really do nothing. Just go through the loop again.
        Serial.print(” Go Right”);
      } else if ((sensorvalue > sensorright) || (sensorright < mintomove)) {
        // Looks like the middle is the right place to be because the left and right
        //   were less than the mintomove.
        // Go back to the center.
        Serial.print(” Recenter”);
        moveleft(MOVEDUR);
        // We’re back at the center, so now we should go forward a little bit.
        moveforward(MOVEDUR);
        Serial.print(” Forward”);
      } else {
        // Looks like left right and center we’re all pretty close, go right
        Serial.print(” Seek Right”);
        moveright(MOVEDUR);
      }
    }



    //

    // Check to see if there is IR anywhere.
    if (sensorvalue < sensormin) sensormin = sensorvalue;
    if (sensorvalue > sensormax) sensormax = sensorvalue;

    // Remember our last value.
    sensorlast = sensorvalue;
  } else {
    movestop();
    Serial.print(” Off!”);
  }
  Serial.println(“…”);
  delay(1000); // Delay
}