The Robot Builder's Shield

If you want to build robots with Arduino you need to buy 2 shields:

  • a motor shield that will also have a prototyping area
  • a sensor shield
    Of course, you can use the prototyping area on the motor shield to make the connection pins for the sensors.

But what if you’re a beginner and want to build a robot and experiment with servos, motors, sensors, buzzer, LEDs, and other goodies? How many shields you need to buy or what is the perfect buy to have it all?

Well, you can get the Robot Builder’s Shield and start experimenting!

Features:

  • 6 sensor pins (all analog pins, power from Arduino regulated 5V)
  • 6 servo pins (digital pins 8-13, power from the battery connector)
  • a SN754410 H-bridge to drive 2 DC motors (digital pins 4-7, uses Timer0 for PWM, power from battery connector)
  • serial connector (for attaching a serial LCD or anything else)
  • connector for separate battery for motors and servos
  • space for a mini breadboard for experimenting
  • digital pins 2 and 3 (external interrupts) available for encoders

Here are some pictures:
!(http://seriousrobotics.files.wordpress.com/2010/09/dsc00638.jpg?w=300&h=225"alt="" width=“300” height=“255”)
!(http://seriousrobotics.files.wordpress.com/2010/09/dsc00639.jpg?w=300&h=225"alt="" width=“300” height=“255”)
!(http://seriousrobotics.files.wordpress.com/2010/09/dsc00640.jpg?w=300&h=225"alt="" width=“300” height=“255”)
!(http://seriousrobotics.files.wordpress.com/2010/09/dsc00642.jpg?w=300&h=225"alt="" width=“300” height=“255”)

I can sell only the PCB at the moment for $5 plus shipping. The parts will be available soon, but until then, you can get them from SparkFun:

1x https://www.sparkfun.com/commerce/product_info.php?products_id=10007 Arduino stackable headers
1x https://www.sparkfun.com/commerce/product_info.php?products_id=116 pin headers
3x https://www.sparkfun.com/commerce/product_info.php?products_id=8432 screw terminal
1x https://www.sparkfun.com/commerce/product_info.php?products_id=7938 16 pin socket
1x https://www.sparkfun.com/commerce/product_info.php?products_id=315 SN754410
1x https://www.sparkfun.com/commerce/product_info.php?products_id=8375 cap 0.1uF
1x https://www.sparkfun.com/commerce/product_info.php?products_id=7916 mini breadboard
Total is: $14.90 plus shipping. I’ll try to at least match that price if not better.

Nice job! I would have made the breadboard area a prototyping area. By adding some holes.

Thanks! I will add that for sure in the next revision. Now I have to sell these to be able to put more money into production... I mean, $5 it's a lot less than what anyone can spend on BatchPCB for one piece...

I agree with the suggestion for adding a prototyping area. It increases the PCB cost a bit (extra drilling), but it's a nice option to have.

And kudos for separating the servo power supply: the otherwise-very-nice Adafruit motor shield doesn't do that, and it's annoying to have to do special wiring to work around it.

yes, separating servo power from motor power is a very good idea the motorshield from adafruit does not do that I Always have to break servo connector to power them.

Actually, the motors and servos share the same power connector... Which is not connected to Arduino Vin. The idea was that Arduino needs at least 7.2V, which most low power motors are fine with, but the servos may be damaged. So having a separate power supply of 6V for motors and servos seemed a good idea to have.

Now you guys let me know if I should separate the motors and servos, so the user will have the option to connect only the servos to a 6V battery and the motors to the Arduino Vin or both to the same battery.

Another option would be to have a LDO regulator that will feed 5V to Arduino and use a single 6V battery for the whole system. The only problem with this option is that the user has to be very careful to not accidentally swap the power connections or he will fry all electronics.

I would also replace the screw terminal for power with a polarized Molex connector and provide a 6" power cable with a mating housing in the kit.

There is a lot of motor of more than 7,2V. 9,6v is a good example. Also you could add a regulator to give power to servos from the power of motor

Yes, that's true.

I designed this shield as an aid to a beginner robot builder that wants to use Arduino instead of Picaxe for example. With the SN754410 H-bridge only low power motors can be used, for example all plastic case geared motors from Solarbotics and part of the micro metal geared motors from Pololu. For more beefier motors, dedicated drivers are needed but in that case I would not use this shield.

Anything extra added to the shield makes it bigger or requires SMD devices that are hard to solder by the user. Anything ready made is expensive, unless you mass produce it.

I think it's hard to create a solution for all user preferences. But this is a good thing, because more products have it's own slots or user markets.

Hi, This sure looks like a nice shield, and for 5 bucks, well, you can't beat it. My minimum cost for pcbs is a lot more than that.

I have done quite a bit with the Arduino, but no motors. I have an application for a client that will required some robot "stuff", and I don't really know what I need (drivers, etc) for various strength motors. Do you know of a good website that has some tutorial and info on setting this type of stuff up? thanks

You can read this tutorial: http://www.societyofrobots.com/member_tutorials/node/159 And this page: http://www.barello.net/Papers/Motion_Control/index.htm

Here is the setup for the H-bridge:

#define Motor_1_PWM   5 // digital pin 5    // Right Motor
#define Motor_1_Dir   4 // digital pin 4
#define Motor_2_PWM   6 // digital pin 6   // Left Motor
#define Motor_2_Dir   7 // digital pin 7

void setup() { 
  // set motor pins as output and LOW so the motors are breaked
  pinMode(Motor_1_PWM, OUTPUT);
  pinMode(Motor_1_Dir, OUTPUT);
  pinMode(Motor_2_PWM, OUTPUT);
  pinMode(Motor_2_Dir, OUTPUT);

  digitalWrite(Motor_1_PWM, LOW);
  digitalWrite(Motor_1_Dir, LOW);
  digitalWrite(Motor_2_PWM, LOW);
  digitalWrite(Motor_2_Dir, LOW);
}

Here are the motor functions:

void Forward(){
  digitalWrite(Motor_1_Dir, LOW); // forward
  digitalWrite(Motor_2_Dir, LOW); // forward
  analogWrite(Motor_1_PWM, speed1); // 
  analogWrite(Motor_2_PWM, speed2); //
  return;
}

void Reverse(){
  digitalWrite(Motor_1_Dir, HIGH); // reverse
  digitalWrite(Motor_2_Dir, HIGH); // reverse
  analogWrite(Motor_1_PWM, 255-speed1); // 
  analogWrite(Motor_2_PWM, 255-speed2); //
  return;
}

void Right(){
  digitalWrite(Motor_1_Dir, HIGH); // reverse
  digitalWrite(Motor_2_Dir, LOW); // forward
  analogWrite(Motor_1_PWM, 255-speed1); // 
  analogWrite(Motor_2_PWM, speed2); //
  return;
}

void Left(){
  digitalWrite(Motor_1_Dir, LOW); // forward
  digitalWrite(Motor_2_Dir, HIGH); // reverse
  analogWrite(Motor_1_PWM, speed1); // 
  analogWrite(Motor_2_PWM, 255-speed2); //
  return;
}

void Stop()
{
  digitalWrite(Motor_1_PWM, LOW);
  digitalWrite(Motor_1_Dir, LOW);
  digitalWrite(Motor_2_PWM, LOW);
  digitalWrite(Motor_2_Dir, LOW);
  return;
}

I have built a robot with another board I designed just for robots that has the same functionality as the Robot Builder’s Shield. The difference between this special board called the uBotino and the RBS are:

  • uBotino has only 5 servo connectors instead of 6
  • pin D8 goes to the H-bridge instead of pin D4
  • uBotino has an extra digital sensor connector, pin D4

The robot has the following features:

  • digital bumper sensors on pins D18 and D19
  • digital ultrasonic sensor on pin D14
  • digital IR remote sensor on pin D15
  • panning sevo on pin D9
  • 2 geared motors (pins D5-D8)
  • speaker on pin D4
  • IR remote control for driving and to play a song
  • autonomous object avoidance

Here is a video of the robot:

Here is the code (divided in 2 parts):

// Bot4Julia, a simple Start Here style robot, made with 2 CDs,
// 2 GM17 motors and wheels, one servo, one SeeedStudio US sensor,
// one battery box, one uBotino board
//
// uBotino board pinout:
//
// uBotino Funct Arduino  ATmega168      Arduino Funct uBotino
//                       +-----\/----+
//          Reset       1| PC6   PC5 |28  D19 A5  SCL  RightBumper
//          Rx    D0    2| PD0   PC4 |27  D18 A4  SDA  LeftBumper
//          Tx    D1    3| PD1   PC3 |26  D17 A3        
//          Int0  D2    4| PD2   PC2 |25  D16 A2        
//          Int1  D3    5| PD3   PC1 |24  D15 A1        IR sensor
//      Spk       D4    6| PD4   PC0 |23  D14 A0        Ping
//                      7| VCC   GND |22  
//                      8| GND  AREF |21  
//          Xtal        9| PB6  AVCC |20  
//          Xtal       10| PB7   PB5 |19  D13      SCK  LED      
//      M1A OC0B  D5   11| PD5   PB4 |18  D12      MISO 
//      M2A OC0A  D6   12| PD6   PB3 |17  D11 OC2A MOSI       
//      M2B       D7   13| PD7   PB2 |16  D10 OC1B      
//      M1B       D8   14| PB0   PB1 |15  D 9 OC1A      Pan servo
//                       +-----------+
//


#include <Servo.h>
#include <Speaker.h>

//Inputs/outputs
//#define Encoder_1_ChA  2 // digital pin 2    // Right Encoder
//#define Encoder_2_ChA  3 // digital pin 3    // Left Encoder

#define Motor_1_PWM   5 // digital pin 5    // Right Motor
#define Motor_1_Dir   8 // digital pin 8
#define Motor_2_PWM   6 // digital pin 6   // Left Motor
#define Motor_2_Dir   7 // digital pin 7

#define PingPin  14     // digital pin 14 (analog pin 0)
#define IR_Pin  15      // digital pin 15 (analog pin 1)
#define LeftBumper 18
#define RightBumper 19
#define PanPin    9     
#define SpeakerPin 4
#define LedPin 13

#define center 90

// Remote control buttons
#define btnPower   149
#define btnMute    148
#define btnPrevCh  187
#define btnDown    145
#define btnUp      144
#define btnLeft    147
#define btnRight   146
#define btnPlay    1434
#define btnStop    1432
#define btnRecord  1437
#define btnPause   1433
#define btnRew     1435
#define btnFwd     1436
#define btnInfo    186
#define btnSleep   182
#define btnInput   165
#define btnEnter   139
#define btn0       137
#define btn1       128
#define btn2       129
#define btn3       130
#define btn4       131
#define btn5       132
#define btn6       133
#define btn7       134
#define btn8       135
#define btn9       136

//Variables
byte dir=0;
byte speed1=230;
byte speed2=255;
int turn90=400;
int turn45=200;
int stopTime=200;
int USdistance=0;
int treshold=20; //20cm min distance

// remote control variables
#define start_bit  2200            // Start bit threshold (Microseconds)
#define bin_1  1000              // Binary 1 threshold (Microseconds)
#define bin_0  400              // Binary 0 threshold (Microseconds)

Servo Pan; 
Speaker speaker = Speaker(SpeakerPin); 
//-----------------------------------------------------------------------------


void setup() { 
   
  // set motor pins as output and LOW so the motors are breaked
  pinMode(Motor_1_PWM, OUTPUT);
  pinMode(Motor_1_Dir, OUTPUT);
  pinMode(Motor_2_PWM, OUTPUT);
  pinMode(Motor_2_Dir, OUTPUT);
  Stop();

  pinMode(IR_Pin, INPUT); // uses an analog pin so it has to be declared as digital in

  pinMode(PingPin, OUTPUT); 
  digitalWrite(PingPin, LOW);
  
  pinMode(LeftBumper, INPUT); 
  digitalWrite(LeftBumper, HIGH); //turn on pull ups
  pinMode(RightBumper, INPUT); 
  digitalWrite(RightBumper, HIGH); //turn on pull ups

  Pan.attach(PanPin);
  Pan.write(center); //90
  StepDelay();

  pinMode(SpeakerPin, OUTPUT); 
  speaker.Beep();

  pinMode(LedPin, OUTPUT);      
  digitalWrite(LedPin, LOW);  

  Serial.begin (19200);
  Serial.println("start");

  Forward();
} 

void loop(){
  Drive();
  Get_IR_Command();
  //square();
}

void Get_IR_Command() {
  int key = getIRKey();                //Fetch the key
  Serial.print("Key ");
  Serial.println(key);
  switch (key) {    
    case btnLeft:
      Left();
      delay(turn45);
      Stop();
      break;
      
    case btnRight:    
      Right();
      delay(turn45);
      Stop();
      break;
      
    case btnUp:
      Forward();
      break;  
      
    case btnDown:
      Reverse();
      break;

    case btnStop:
      Stop();
      break;

    case btnMute:
      speaker.PlayMelody();
      break;

  }  
  //StepDelay();
  return;
}

//--------------------------
int getIRKey() {
  int data[12];
  int newdata=pulseIn(IR_Pin, LOW, 100000);
  while(newdata>0 && newdata<start_bit) { //Wait for a start bit
    newdata=pulseIn(IR_Pin, LOW, 100000);
  }
  if (newdata==0) return 0;
  for(int i=0;i<11;i++){
    data[i] = pulseIn(IR_Pin, LOW, 100000);   //Start measuring bits, we only want low pulses
  }
  speaker.Beep();
  for(int i=0;i<11;i++) {            //Parse them
    if(data[i] > bin_1) {            //is it a 1?
      data[i] = 1;
    }  else {
      if(data[i] > bin_0) {            //is it a 0?
        data[i] = 0;
      } else {
       data[i] = 2;                  //Flag the data as invalid; I don't know what it is!
      }
    }
  }

   for(int i=0;i<11;i++) {            //Pre-check data for errors
    if(data[i] > 1) {
      return -1;                    //Return -1 on invalid data
    }
  }

  int result = 0;
  int seed = 1;
  for(int i=0;i<11;i++) {            //Convert bits to integer
    if(data[i] == 1) {
      result += seed;
    }
    seed = seed * 2;
  }
  return result;                  //Return key number
}

void square(){
  Forward();
  delay(2000);
  Stop();
  delay(stopTime);
  Right();
  delay(turn90);
  Stop();
  delay(stopTime);
  Forward();
  delay(2000);
  Stop();
  delay(stopTime);
  Right();
  delay(turn90);
  Stop();
  delay(stopTime);
  Forward();
  delay(2000);
  Stop();
  delay(stopTime);
  Right();
  delay(turn90);
  Stop();
  delay(stopTime);
  Forward();
  delay(2000);
  Stop();
  delay(stopTime);
  Right();
  delay(turn90);
  Stop();
  delay(stopTime);
}

void Drive(){
  if (digitalRead(LeftBumper)==LOW){
  digitalWrite(LedPin, HIGH);  
    Stop();
    speaker.Beep();
    StepDelay();
  digitalWrite(LedPin, LOW);  
    Reverse();
    StepDelay();
    Stop();
    StepDelay();
    Right();
    delay(turn45); //turn45
    Stop();
    StepDelay();
    Forward();
  }
  if (digitalRead(RightBumper)==LOW){
  digitalWrite(LedPin, HIGH);  
    Stop();
    speaker.Beep();
    StepDelay();
  digitalWrite(LedPin, LOW);  
    Reverse();
    StepDelay();
    Stop();
    StepDelay();
    Left();
    delay(turn45); //turn45
    Stop();
    StepDelay();
    Forward();
  }
  USdistance=Read_Ping_Sensor();
  Serial.print("USdistance ");
  Serial.println(USdistance);
  if (USdistance<10){
    Stop();
    speaker.Beep();
    StepDelay();
    TurnAround();
  }
  if (USdistance<treshold){
    Stop();
    speaker.Beep();
    StepDelay();
    Avoid();
    Forward();
  }
  delay(50);
}

void TurnAround(){
    Reverse();
    Pan.write(center);
    StepDelay();
    Stop();
    Left();
    delay(turn90);
    delay(turn90);
    Stop();
    StepDelay();
    Forward();
}

void Avoid(){
  int prev=0;
  dir=2;
  for (byte i=0; i<5; i++){
    Pan.write(i*45);
    StepDelay();
    StepDelay();
    USdistance=Read_Ping_Sensor();
    if (USdistance>prev){
      dir=i;
      prev=USdistance;
    }
  }
  Pan.write(center);
  StepDelay();
  switch (dir){
    case 0:
      Right();
      delay(turn90);
      Stop();
      speaker.Beep();
      StepDelay();
      break;
    case 1:
      Right();
      delay(turn90); //turn45
      Stop();
      speaker.Beep();
      StepDelay();
      break;
    case 2:
      Forward();
      break;
    case 3:
      Left();
      delay(turn90); //turn45
      Stop();
      speaker.Beep();
      StepDelay();
      break;
    case 4:
      Left();
      delay(turn90);
      Stop();
      speaker.Beep();
      StepDelay();
      break;
  }
  delay(500);
}  

// Read Sensors
int Read_Ping_Sensor(){
  //digitalWrite(LedPin, HIGH);  
  int cm=0;
  //trigger the sensor
  unsigned long value = 0;
  pinMode(PingPin, OUTPUT);
  digitalWrite(PingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(PingPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(PingPin, LOW);
  //receive the echo
  pinMode(PingPin, INPUT);
  digitalWrite(PingPin, HIGH); // turn on pull up resistor
  value = pulseIn(PingPin, HIGH);
  value=value/58;
  cm=int(value);
  //digitalWrite(LedPin, LOW);  
  return cm;
}

void StepDelay() {
    for (byte t=0; t<10; t++){
      //SoftwareServo::refresh();
      delay(20);
    }
}

And the second part of the code:

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void Forward(){
  digitalWrite(Motor_1_Dir, LOW); // forward
  digitalWrite(Motor_2_Dir, LOW); // forward
  analogWrite(Motor_1_PWM, speed1); // 
  analogWrite(Motor_2_PWM, speed2); //
  return;
}

void Reverse(){
  digitalWrite(Motor_1_Dir, HIGH); // reverse
  digitalWrite(Motor_2_Dir, HIGH); // reverse
  analogWrite(Motor_1_PWM, 255-speed1); // 
  analogWrite(Motor_2_PWM, 255-speed2); //
  return;
}

void Right(){
  digitalWrite(Motor_1_Dir, HIGH); // reverse
  digitalWrite(Motor_2_Dir, LOW); // forward
  analogWrite(Motor_1_PWM, 255-speed1); // 
  analogWrite(Motor_2_PWM, speed2); //
  return;
}

void Left(){
  digitalWrite(Motor_1_Dir, LOW); // forward
  digitalWrite(Motor_2_Dir, HIGH); // reverse
  analogWrite(Motor_1_PWM, speed1); // 
  analogWrite(Motor_2_PWM, 255-speed2); //
  return;
}

void Stop()
{
  digitalWrite(Motor_1_PWM, LOW);
  digitalWrite(Motor_1_Dir, LOW);
  digitalWrite(Motor_2_PWM, LOW);
  digitalWrite(Motor_2_Dir, LOW);
  return;
}

I have used a custom library called Speaker that you can download from here: Speaker.zip

Nice job, great video. I am currently working on a similar robot with the Adafruit motor shield.

Hi Ro-Bot-X

Nice bot I love the way the Ping sensor is mounted on the servo :) :)

Thanks for the comments guys.

I've been busy with a lot of stuff and I did not test the shield completely. And someone noticed a problem. A signal trace touches a pad on the H-bridge, causing a motor to work only one way. To correct this you need to cut along the trace (see the picture below) using a utility knife. Only the top layer is affected, so when you solder the socket on the bottom layer, you will not re-create the short.

Apologies for any inconvenience.

I wanted to make a small Arduino robot using an original Duemilanove and the Robot Builder's Shield. But I also wanted to have a chassis plate so I can mount the parts on. I started to play with the parts to see how they will fit together and finally came up with this design that looked so compact that it became the name of the robot. After a little AutoCAD drawing, some CamBam fiddling and nerve braking CNC-ing, there is the first imperfect chassis plates: I threaded the holes with a #4-40 tap (that was the only one I had, I'll have to find a M3 tap). Then I started to mount the motors, battery holders, the ball caster, the Arduino board and the servo. Then I had to take off the Arduino board because I needed to add a bigger hole to pass the motor wires through. I installed the shield on top and soldered a switch to the solid wire I used to route the power from the power connector on the shield to the Vin pin so the Arduino board will get the power from there. Since I am using 6V, the resulted regulated voltage is 4.88V. Unfortunately I can't change the SMD voltage regulator with a LDO version onboard the Arduino, too small to solder by hand and of course, I don't have a proper SMD regulator. But I think it'll work just fine.

Here are more pictures of the robot, a video will follow soon:

That's looking really great! :-)

Can't wait to see the video!

Here is the video:

http://www.youtube.com/watch?v=cDczbYl1dWs

And the code:

// Compact, a small Arduino (Duemilanove) robot
// 2 Pololu micro motors 100:1 and small wheels,
// one HXT900 micro servo, one Sharp IR sensor,
// 2 AAA battery holders, the Robot Builder's Shield
//
// Arduino pinout:
//
// Shield  Funct Arduino  ATmega168      Arduino Funct Shield
//                       +-----\/----+
//          Reset       1| PC6   PC5 |28  D19 A5  SCL  
//          Rx    D0    2| PD0   PC4 |27  D18 A4  SDA  
//          Tx    D1    3| PD1   PC3 |26  D17 A3        
//          Int0  D2    4| PD2   PC2 |25  D16 A2        
//          Int1  D3    5| PD3   PC1 |24  D15 A1        
//      M1B       D4    6| PD4   PC0 |23  D14 A0        IR sensor
//                      7| VCC   GND |22  
//                      8| GND  AREF |21  
//          Xtal        9| PB6  AVCC |20  
//          Xtal       10| PB7   PB5 |19  D13      SCK  LED      
//      M1A OC0B  D5   11| PD5   PB4 |18  D12      MISO Pan servo
//      M2A OC0A  D6   12| PD6   PB3 |17  D11 OC2A MOSI       
//      M2B       D7   13| PD7   PB2 |16  D10 OC1B      
//                D8   14| PB0   PB1 |15  D 9 OC1A      
//                       +-----------+
//


#include <Servo.h>

//Inputs/outputs
#define Motor_1_PWM   5 // digital pin 5    // Right Motor
#define Motor_1_Dir   4 // digital pin 4
#define Motor_2_PWM   6 // digital pin 6   // Left Motor
#define Motor_2_Dir   7 // digital pin 7

#define IR_Pin  14      // digital pin 14 (analog pin 0)
#define PanPin  12
#define LedPin  13

#define SR 1 //Sharp Short Range sensor
#define MR 2 //Sharp Medium Range sensor
#define LR 3 //Sharp Long Range sensor

#define center 90

//Variables
byte dir=0;
byte speed1=250;
byte speed2=255;
int turn90=110;
int turn45=55;
int straight=500;
int stopTime=200;
int IRdistance=0;
int treshold=20; //20cm min distance


Servo Pan; 
//-----------------------------------------------------------------------------


void setup() { 
   
  // set motor pins as output and LOW so the motors are breaked
  pinMode(Motor_1_PWM, OUTPUT);
  pinMode(Motor_1_Dir, OUTPUT);
  pinMode(Motor_2_PWM, OUTPUT);
  pinMode(Motor_2_Dir, OUTPUT);
  Stop();

  Pan.attach(PanPin);
  Pan.write(center); //90
  StepDelay();

  pinMode(LedPin, OUTPUT);      
  digitalWrite(LedPin, LOW);  

  Serial.begin (19200);
  Serial.println("start");

  Forward();
} 

void loop(){
  Drive();
  //square();
}

void square(){
  Forward();
  delay(straight);
  Stop();
  delay(stopTime);
  Right();
  delay(turn90);
  Stop();
  delay(stopTime);
  Forward();
  delay(straight);
  Stop();
  delay(stopTime);
  Right();
  delay(turn90);
  Stop();
  delay(stopTime);
  Forward();
  delay(straight);
  Stop();
  delay(stopTime);
  Right();
  delay(turn90);
  Stop();
  delay(stopTime);
  Forward();
  delay(straight);
  Stop();
  delay(stopTime);
  Right();
  delay(turn90);
  Stop();
  delay(stopTime);
}

//--------------------------
void Drive(){
  IRdistance=Read_Sharp_Sensor(MR, IR_Pin);
  Serial.print("IRdistance ");
  Serial.println(IRdistance);
  if (IRdistance<10){
    Stop();
    StepDelay();
    TurnAround();
  }
  if (IRdistance<treshold){
    Stop();
    StepDelay();
    Avoid();
    Forward();
  }
  delay(50);
}

void TurnAround(){
    Reverse();
    Pan.write(center);
    StepDelay();
    Stop();
    Left();
    delay(turn90);
    delay(turn90);
    Stop();
    StepDelay();
    Forward();
}

void Avoid(){
  int prev=0;
  dir=2;
  for (byte i=0; i<5; i++){
    Pan.write(i*45);
    StepDelay();
    StepDelay();
    IRdistance=Read_Sharp_Sensor(MR, IR_Pin);
    if (IRdistance>prev){
      dir=i;
      prev=IRdistance;
    }
  }
  Pan.write(center);
  StepDelay();
  switch (dir){
    case 0:
      Right();
      delay(turn90);
      Stop();
      StepDelay();
      break;
    case 1:
      Right();
      delay(turn90); //turn45
      Stop();
      StepDelay();
      break;
    case 2:
      Forward();
      break;
    case 3:
      Left();
      delay(turn90); //turn45
      Stop();
      StepDelay();
      break;
    case 4:
      Left();
      delay(turn90);
      Stop();
      StepDelay();
      break;
  }
  delay(500);
}  

// Read Sensors
int Read_Sharp_Sensor(byte model, byte pin) {
  int value = 0;
  value = analogRead(pin);
  switch (model) {
    case SR: //short range, aka GP2D120 (4-30cm)
      return (2914/(value+5))-1; 
      break;
    case MR: //medium range, aka GP2D12 (10-80cm)
      return 5*1384.4*pow(value,-.9988); //I had to multiply by 5, different sensor
      break;
    case LR: //long range, aka GP2Y0A02YK (20-150cm)
      return 11441*pow(value,-.9792); 
      break;
  }
}

void StepDelay() {
    for (byte t=0; t<10; t++){
      delay(20);
    }
}


//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void Forward(){
  digitalWrite(Motor_1_Dir, LOW); // forward
  digitalWrite(Motor_2_Dir, LOW); // forward
  analogWrite(Motor_1_PWM, speed1); // 
  analogWrite(Motor_2_PWM, speed2); //
  return;
}

void Reverse(){
  digitalWrite(Motor_1_Dir, HIGH); // reverse
  digitalWrite(Motor_2_Dir, HIGH); // reverse
  analogWrite(Motor_1_PWM, 255-speed1); // 
  analogWrite(Motor_2_PWM, 255-speed2); //
  return;
}

void Right(){
  digitalWrite(Motor_1_Dir, HIGH); // reverse
  digitalWrite(Motor_2_Dir, LOW); // forward
  analogWrite(Motor_1_PWM, 255-speed1); // 
  analogWrite(Motor_2_PWM, speed2); //
  return;
}

void Left(){
  digitalWrite(Motor_1_Dir, LOW); // forward
  digitalWrite(Motor_2_Dir, HIGH); // reverse
  analogWrite(Motor_1_PWM, speed1); // 
  analogWrite(Motor_2_PWM, 255-speed2); //
  return;
}

void Stop()
{
  digitalWrite(Motor_1_PWM, LOW);
  digitalWrite(Motor_1_Dir, LOW);
  digitalWrite(Motor_2_PWM, LOW);
  digitalWrite(Motor_2_Dir, LOW);
  return;
}