YourDuino-Basic-Robot-Kit-Software-Version2

From ArduinoInfo
Jump to: navigation, search
print this page

YourDuino-Basic-Robot-Kit V2 -Software-Version2

How-To Instructions for the mechanical build of the robot are HERE

We now have more full-featured software for THIS Robot. The versions at the bottom of this page run the Robot, using the Servo to "Look Around" with the Ultrasonic Sensor for possible collisions to the Left, Center and Right. The Robot then does a move to turn away or turn around and continue. NOTE: You need the libraries downloaded and installed (see below).

TEST ROBOT MOVES AND CREATE YOUR OWN ROBOT CONTROL PROGRAMS</span>

We now have a page that shows details of making your own Robot Control Programs.</span>CLICK  to go there. BUT make sure you have tested the different parts first.</span>

TEST THE DIFFERENT PARTS FIRST!

It's very hard to debug problems when many things are happening at once. Just below, this page has example Arduino "Software Sketches" you should use to test the different parts of the YourDuino Basic Robot Kit separately. This the best time to find any wiring or other problems before you try to run all the parts at once. There are separate Software Sketches for these robot parts:
  • YourDuinoRobo1 (The usual BLINK program)</span>
  • Servo</span>
  • Ultrasonic Distance Sensor</span>
  • Motor Driver</span>


Getting Started:

If you do not yet have the Arduino Software installed GO HERE to get started. If you have an earlier version, we suggest you update to Arduino version 1.6.1 which has the new easy Library Install feature.

Using LIBRARIES:</span>

Arduino has many great helpers who contribute software and hardware solutions. We will use software that others wrote and put into Libraries to control the parts of your robot.</span>
See more about Libraries HERE:</span>
And.. we suggest you use the new easier Library Install Method HERE:</span>

Here are the libraries we will use:</span>
  1. SERVO Library (Comes with regular Arduino software)
  2. NEWPING Library (For Ultrasonic Distance Sensor) (downloaded)
    1. Download HERE
  3. MotorDriver Library (Controls the motors) (downloaded)
    1. Download HERE
  4. SM (State Machine) Library used in Autonomous Robot software at the bottom of this page.
    1. Download HERE

Get those 3 libraries downloaded and installed, then continue with tests and autonomous robot software below.


GETTING SOFTWARE SKETCHES:

You will Copy and Paste the Software Sketches below to test and run your robot. Here's how:
  • On this page, position your mouse cursor just before the beginning of a sketch, then hold the left button down and move slowly down over the sketch and over the end " //*( THE END )*"</span>
  • Press <Ctrl> C to </span>COPY. (<Command> V on MAC)
  • Switch to the Arduino IDE software. </span>
  • Click on File > New (You will get a new window)</span>
  • Use your mouse cursor and left button to highlight the small example shown there. Leave it highlighted so you will replace it.</span>
  • PASTE the sketch by pressing <Ctrl> V (<Command> V on MAC)
  • Check that the beginning and end of the sketch are right.</span>
  • Click </span>Verify on the IDE and make sure there are no errors. If problems, see "About Libraries" Here.
  • Click </span>Upload** on the IDE with your USB cable connected and the right Port selected. It should say "Compiling Sketch" and then "Uploading Sketch" and then "Upload Complete". You should see the Tx and Rx LEDs on the RoboRed board blink for a second or two during the actual upload..
  • Your sketch should start running!</span>


TEST SERVO MOTION:

The SERVO library is part of the regular Arduino software installation. It includes "Examples". We will us the example called "Sweep" which will move the servo through its range of movement. Copy and paste this Software Sketch to test the servo.

Plug in the USB Cable to upload this software sketch. You can run this test from the USB but later when multiple things are running you'll need to use the battery case power.

After uploading this Software Sketch, the servo should "look around" back and forth. It will keep going until you unplug the YourDuino or upload a different Sketch. (When you want to stop the servo motion, just upload Blink again.)
/* YourDuino Basic Robot Kit V2: Test Servo movement - WHAT IT DOES: Tests the servo by commanding it to go to several different directions to "Look Around". You may want to reposition the servo arm on the servo to get the positions to look around correctly. - SEE the comments after "//" on each line below - CONNECTIONS: - - - V2.10 11/10/2014 Questions: terry@yourduino.com */ /*-----( Import needed libraries )-----*/ #include <Servo.h> /*-----( Declare Constants and Pin Numbers )-----*/ #define SERVO_PIN 11 // Servo plugs into Pin 11 /*-----( Declare objects )-----*/ Servo myservo; // create servo object to control a servo /*-----( Declare Variables )-----*/ int pos; // variable to store the servo position void setup() /****** SETUP: RUNS ONCE ******/ { myservo.attach(SERVO_PIN); // attaches the servo on pin 11 to the servo object }//--(end setup )--- void loop() /****** LOOP: RUNS CONSTANTLY ******/ { for(pos = 0; pos < 180; pos += 30) // goes from 0 degrees to 180 degrees { // in steps of 30 degree myservo.write(pos); // tell servo to go to position in variable 'pos' delay(1000); // Wait 500ms between pings } for(pos = 180; pos>=1; pos -= 30) // goes from 180 degrees to 0 degrees { myservo.write(pos); // tell servo to go to position in variable 'pos' delay(1000); } }//--(end main loop )--- /*-----( Declare User-written Functions )-----*/ //NONE //*********( THE END )***********







TEST ULTRASONIC DISTANCE SENSOR:


We will use a library called NewPing written by Tim Eckel to run the Ultrasonic Sensor. You will need to download and install the NewPing library.Screen shot 2013-11-12 at 7.41.03 PM.png You can find the NewPing library zip file HERE and add it to your "libraries" folder. Close and restart the Arduino IDE so that the library is recognized. </span>

See more about Libraries HERE
Then run the test sketch below. </span>
Note: The sketch will send the distance measurements it is making to the "Serial Monitor," which is a window you can start by clicking on the "serial monitor" icon at the upper right of the IDE window. Put your hand or other object in front of the Ultrasonic sensor and move it farther and closer and you should see the distance displayed on the Serial Monitor.</span>

/* YourDuino Basic Robot Kit V2: Ultrasonic Sensor Test
 - WHAT IT DOES
   - Tests the operation of the Ultrasonic Sensor
 - SEE the comments after "//" on each line below
 - CONNECTIONS: (suggest cable with 4 wires: Red,Orange,Yellow,Green )
 - Sensor Vcc to RoboRED     Vcc     (Red)
 - Sensor Ground to RoboRED  Gnd     (Green)
 - Sensor Trig to RoboRed    pin 2   (Orange)
 - Sensor Echo to RoboRED    pin 3   (Yellow) 
 - V2.10 11/10/2014
 Questions: terry@yourduino.com */

/*-----( Import needed libraries )-----*/
#include <NewPing.h>

/*-----( Declare Constants and Pin Numbers )-----*/
#define TRIGGER_PIN  2   // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     3   // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

/*-----( Declare objects )-----*/
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
/*-----( Declare Variables )-----*/
unsigned int uS;

void setup()   /****** SETUP: RUNS ONCE ******/
{
  Serial.begin(9600); // Open serial monitor at 9600 baud to see ping results.

}//--(end setup )---


void loop()   /****** LOOP: RUNS CONSTANTLY ******/
{
  delay(50);                      // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
  Serial.print("Ping: ");
  Serial.print(uS / US_ROUNDTRIP_CM); // Convert ping time to distance in cm and print result (0 = outside set distance range)
  Serial.println("cm");

}//--(end main loop )---

/*-----( Declare User-written Functions )-----*/
//NONE

//*********( THE END )***********




TEST MOTOR DRIVER AND MOTORS:


Battery Case Setup:
Insert 6 AA batteries into the battery case. Be careful about the orientation of each battery. Make sure each battery's + end is pushed towards the + contact. You will want to use the small flat screwdriver to push the individual AA cells toward the + connection to make sure. (There is a lot of friction in a new battery case and they might not make contact).

We recommend rechargeable NiMH type batteries. You will also find the small flat screwdriver good to help get batteries OUT of the battery case. They are very tight and secure, but difficult to get in and out.

Start with the battery case UNplugged from the Robo1. Find something to sit your robot on so that it's two big wheels are off the ground.

Download the motor driver library HERE and add it to your "libraries" folder.
See more about Libraries HERE:
Close and restart the Arduino IDE so that the library is recognized.

Cut and paste the test Software Sketch below into a new blank sketch window. Click Verify to make sure the software and libraries are installed correctly. Then click Upload to send it to the Robo1.

Now UNplug the USB cable and plug in the Battery Case cable. (If you have wired in the switch, turn it on.) After several seconds you should see the LEDs on the motor driver board change brightness and one wheel should turn forward, stop and then turn in reverse, followed by the second motor. After a delay the test should run again.
Note: If you have the motors connected properly you should first see the right-hand motor go forward and then reverse, followed by the left-hand motor going forward and then reverse. If a motor is rotating in the wrong direction, check the wiring as shown in the photos, and if necessary reverse the red and black wires where they are connected to the motor driver terminal block.

/* YourDuino.com Example Software Sketch
 MotorDriverDEV
 YD_MotorDriver1 Library Test
 terry@yourduino.com */

/*-----( Import needed libraries )-----*/
#include <YD_MotorDriver1.h>

/*-----( Declare Constants )-----*/
// NOTE: Pins 9 (Motor1) and 10 (Motor2) are predefined, unchangeable
#define  Motor1A  8
#define  Motor1B  7
#define  Motor2C  6
#define  Motor2D  5

#define  RampDelay  250
/*-----( Declare objects )-----*/
YD_MotorDriver1 RobotDriver(Motor1A,Motor1B,Motor2C,Motor2D);
//YD_MotorDriver1 RobotDriver();

/*-----( Declare Variables )-----*/


void setup()   /*----( SETUP: RUNS ONCE )----*/
{
  Serial.begin(115200);
  Serial.println("YourDuino MotorDriver1 Ramp Up / Down Test");
  RobotDriver.init();

}/*--(end setup )---*/


void loop()   /*----( LOOP: RUNS CONSTANTLY )----*/
{
 delay(2000); // Give time to put the robot down!
  Serial.println("Motor 1 0..+400");
  for (int i = 0; i <= 400; i = i + 10)
  {
    RobotDriver.Motor1Speed(i);
    Serial.print(i,DEC);
    Serial.print(" ");
    delay(RampDelay);
  }
  Serial.println();
  delay(2000);

  Serial.println("Motor 1 +400..0..-400");
  for (int i = 400; i >= -400; i = i - 10)
  {
    RobotDriver.Motor1Speed(i);
    Serial.print(i,DEC);
    Serial.print(" ");
    delay(RampDelay);
  }
  Serial.println();
  delay(2000);

  Serial.println("Motor 1 -400..0");
  for (int i = -400; i <= 0; i = i + 10)
  {
    RobotDriver.Motor1Speed(i);
    Serial.print(i,DEC);
    Serial.print(" ");
    delay(RampDelay);
  }
  Serial.println();
  delay(2000);



/*----( Ramp Motor2 Up and Down both directions )-------*/
  Serial.println("Motor 2 0+");
  for (int i = 0; i <= 400; i = i + 10)
  {
    RobotDriver.Motor2Speed(i);
    Serial.print(i,DEC);
    Serial.print(" ");
    delay(RampDelay);
  }
  Serial.println();
  delay(2000);

  Serial.println("Motor 2 +-");
  for (int i = 400; i >= -400; i = i - 10)
  {
    RobotDriver.Motor2Speed(i);
    Serial.print(i,DEC);
    Serial.print(" ");
    delay(RampDelay);
  }
  Serial.println();
  delay(2000);

  Serial.println("Motor 2 -0");
  for (int i = -400; i <= 0; i = i + 10)
  {
    RobotDriver.Motor2Speed(i);
    Serial.print(i,DEC);
    Serial.print(" ");
    delay(RampDelay);
  }
  Serial.println();
  delay(2000);

//  delay(5000);  
}/* --(end main loop )-- */

/*-----( Declare User-written Functions )-----*/


/* ( THE END ) */



TEST AND ADJUST MOTOR SPEEDS:

This test sketch will run the robot in slow, medium and fast speeds. You should place it on a smooth floor if possible. Carpet, especially thick carpet will slow or stop this robot. If you must run it on some carpet, you may have to change the MoveSpeed values to be higher, like SlowMoveSpeed 300, MediumMoveSpeed 360, FastMovespeed 390. But it will only run really well on a smooth floor.

You can adjust the right VS left motor speeds to try to get the robot to move straight ahead. You can hold the robot in both hands, holding the RESET button on the right side of the RoboRED pressed until you put the robot down. Also make sure the caster wheel is pointing straight when you start.

See this part of the test sketch:

#define SlowMoveSpeed 280
#define SlowMoveAdjust 5 Adjust for straight move: - Left + Right??</span>

#define MediumMoveSpeed 300
#define MediumMoveAdjust 5 </span> Adjust for straight move: - Left + Right

#define FastMoveSpeed 350
#define FastMoveAdjust 5 // Adjust for straight move: - Left + Right


After you have found the best MoveAdjust values, write them down (or later cut and paste) for use in the other sketches.
/* YourDuino Basic Robot Kit V2 Motor Speed Test http://yourduino.com/sunshop2/index.php?l=product_detail&p=400 - WHAT IT DOES: - Runs the robot motors at different speeds. Helps adjust Right-Left offsets - SEE the comments after "//" on each line below - CONNECTIONS: Pins 9 (Motor1 PWM) and 10 (Motor2 PWM) are predefined, unchangeable // Label--Arduino Pin-- Motor Driver Pin Motor1A pin 8 // INA Motor1B pin 7 // INB Motor2C pin 6 // INC Motor2D pin 5 // IND - V2.10 11/10/2014 Questions: terry@yourduino.com */ /*-----( Import needed libraries )-----*/ #include <YD_MotorDriver1.h> // For control of the two DC Motors /*-----( Declare Constants and Pin Numbers )-----*/ // NOTE: Pins 9 (Motor1) and 10 (Motor2) are predefined, unchangeable #define Motor1A 8 #define Motor1B 7 #define Motor2C 6 #define Motor2D 5 #define StartMoveSpeed 200 // Motor Driver value for start of motion #define SlowMoveSpeed 280 #define SlowMoveAdjust 5 // Adjust for straight move: - Left + Right?? #define MediumMoveSpeed 300 #define MediumMoveAdjust 5 // Adjust for straight move: - Left + Right #define FastMoveSpeed 350 #define FastMoveAdjust 5 // Adjust for straight move: - Left + Right /*-----( Declare objects )-----*/ YD_MotorDriver1 RobotDriver(Motor1A,Motor1B,Motor2C,Motor2D); // Set pins void setup() /******************* SETUP: RUNS ONCE *****************/ { Serial.begin(115200); delay(1000); Serial.println("YourDuino Robot Kit V2 Motor Speed Test"); //--NOTE: Motor Pins set to OUTPUT by their libraries RobotDriver.init(); }//--(end setup )--- /************** LOOP: RUNS CONSTANTLY **************************/ void loop() { Serial.println("Forward Slow Speed"); ForwardSlow(); delay(5000); Stop(); Serial.println("---STOP---"); delay(5000); Serial.println("Forward Medium Speed"); ForwardMedium(); delay(5000); Stop(); Serial.println("---STOP---"); delay(5000); Serial.println("Forward Fast Speed"); ForwardFast(); delay(5000); Stop(); Serial.println("---STOP---"); delay(10000); Serial.println("---END---"); }//--(end main loop )--- /*----------------( Declare User-written Functions )---------------*/ //------( MOTOR CONTROL FUNCTIONS )---------------- void ForwardSlow() { RobotDriver.Motor1Speed(SlowMoveSpeed + SlowMoveAdjust); RobotDriver.Motor2Speed(SlowMoveSpeed - SlowMoveAdjust); } /*---------------------------*/ void ForwardMedium() { RobotDriver.Motor1Speed(MediumMoveSpeed + MediumMoveAdjust); RobotDriver.Motor2Speed(MediumMoveSpeed - MediumMoveAdjust); } /*---------------------------*/ void ForwardFast() { RobotDriver.Motor1Speed(FastMoveSpeed + FastMoveAdjust); RobotDriver.Motor2Speed(FastMoveSpeed - FastMoveAdjust); } /*---------------------------*/ void BackwardSlow(int HowMuch) { RobotDriver.Motor1Speed(- SlowMoveSpeed ); RobotDriver.Motor2Speed(- SlowMoveSpeed ); delay(HowMuch); Stop(); } /*---------------------------*/ void BackwardMedium(int HowMuch) { RobotDriver.Motor1Speed(- MediumMoveSpeed); RobotDriver.Motor2Speed(- MediumMoveSpeed); delay(HowMuch); Stop(); } /*---------------------------*/ void BackwardFast(int HowMuch) { RobotDriver.Motor1Speed(- FastMoveSpeed); RobotDriver.Motor2Speed(- FastMoveSpeed); delay(HowMuch); Stop(); } /*---------------------------*/ void Stop() { RobotDriver.Motor1Speed(0); RobotDriver.Motor2Speed(0); } /*---------------------------*/ void SpinLeft(int HowMuch) { RobotDriver.Motor1Speed(MediumMoveSpeed); RobotDriver.Motor2Speed(- MediumMoveSpeed); delay(HowMuch); Stop(); } /*---------------------------*/ void SpinRight(int HowMuch) { RobotDriver.Motor1Speed(MediumMoveSpeed); RobotDriver.Motor2Speed(- MediumMoveSpeed); delay(HowMuch); Stop(); } /*---------------------------*/ //*********( THE END )***********


TEST ROBOT MOVES AND CREATE YOUR OWN ROBOT CONTROL PROGRAMS

We now have a page that shows details of making your own Robot Control Programs.CLICK to go there.



AUTONOMOUS COLLISION-AVOIDING ROBOT SKETCHES:

These are more complex software sketches. IF you want to change them you will need good programming skills.

This assumes you have built the robot with:
  • YourDuinoRoboRED
  • Battery Case (Optional: switch)
  • DC Motors connected to the Motor Controller board
  • Servo mounted in the front of the Robot
  • Ultrasonic Sensor mounted on the Servo to "Look Around"
  • The Motor Driver and NewPing (Ultrasonic) and SM (State Machine) libraries have been installed
  • The Tests above of Servo, Motor Driver and Ultrasonic Sensor are all working


The needed connections are shown in the details at the beginning of the sketch. If you followed the directions On The Build Page you will have the correct wiring.

NOTE: See the MoveAdjust values in the Sketches below to compensate for motor differences to make the robot run straighter. It won't be perfect. (See the MotorSpeed sketch above to find good values.

COLLISION AVOIDANCE 1 (2.10): Stops to look around but easier to see actions

Copy and paste this Sketch into a blank Arduino IDE page, save it with a name, Verify it, and Upload it to the Robot.

Plug in the battery connector, hold the robot in both hands, and put it down in a clear area. It should move forward, stop and look around. The sequence of it's actions are:

  1. Move forward at medium speed for about a second
  2. Stop
  3. Use the servo to point the Ultrasonic Sensor Left, Center, Right
  4. IF something is too close, turn on the Pin 13 LED on the RoboRED and THEN:
    1. Left: Spin Right and then start over.
    2. Center: Spin around backwards and then start over
    3. Right: Spin Left and then start over.
  5. Go to the beginning and start over


Test this by holding your flat hand on the left side of the robot as it stops to look. It should spin right, away from you. Test again on the right side, and then straight ahead.

Add a Buzzer to signal when the robot 'sees' something (Actually, hears a close echo). Find the "Buzzer" module in your kit. See it here: Notice it has a (+) mark (and that lead is longer). Strip off a 2-wire piece from your Rainbow Cable. (We used Black and White). Push the White lead on one end on to the Buzzer (+), then Black on the other lead. REMOVE the white sticker! On the other end, plug the Black wire on Pin 13 [G]round (Blue) and the White wire on Pin 13 [S]ignal (Yellow). Arrange the wire so the buzzer does not bounce are or get in the way. Run the robot again. When a too-close object is found, the Pin 13 LED should light as before AND the Buzzer should sound.


/* YourDuino Basic Robot Kit Collision-avoidance Test
   This version is slower, waits to look around.
   http://yourduino.com/sunshop2/index.php?l=product_detail&p=400 
  - WHAT IT DOES:
 - Runs the robot motors
 - Looks Around with the Servo and Ultrasonic Sensor
 - SEE the comments after "//" on each line below
 NOTE: See the MoveAdjust values below to compensate for motor differences
       to make the robot run straighter.
 - CONNECTIONS:
 Pins 9 (Motor1 PWM) and 10 (Motor2 PWM) are predefined, unchangeable

// Label--Arduino Pin-- Motor Driver Pin  
 Motor1A    pin 8           // INA
 Motor1B    pin 7           // INB          
 Motor2C    pin 6           // INC
 Motor2D    pin 5           // IND
 
 - V2.10 11/10/2014
 Questions: terry@yourduino.com */

/*-----( Import needed libraries )-----*/
#include <YD_MotorDriver1.h>  // For control of the two DC Motors
#include <NewPing.h>          // Runs the Ultrasonic Distance Sensor
#include <SM.h>;              // Implements the State Machine that controls action sequence

/*-----( Declare Constants and Pin Numbers )-----*/

// NOTE: Pins 9 (Motor1) and 10 (Motor2) are predefined, unchangeable
#define  Motor1A  8
#define  Motor1B  7
#define  Motor2C  6
#define  Motor2D  5

#define  RampDelay  10

#define  StartMoveSpeed    200  // Motor Driver value for start of motion

#define  SlowMoveSpeed     280
#define  SlowMoveAdjust     5   // Adjust for straight move: - Left + Right??

#define  MediumMoveSpeed   300
#define  MediumMoveAdjust   5  // Adjust for straight move: - Left + Right

#define  FastMoveSpeed     350
#define  FastMoveAdjust     5  // Adjust for straight move: - Left + Rig

// Ultrasonic Sensor Pins
#define TRIGGER_PIN  2   // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     3   // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 100 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
// Servo
#define SERVO_PIN    11  // The "Look Around" servo

#define PIN13_LED    13  // The onboard LED: Lights when an object is sensed by the Ultrasonic

//----( "TARGET FOUND" DIRECTIONS (index into TargetArray )---------
#define TARGET_FOUND_ANY     0   // Values will be "true" or "false"
#define TARGET_LEFT          1
#define TARGET_LEFT_CENTER   2
#define TARGET_CENTER        3
#define TARGET_RIGHT_CENTER  4
#define TARGET_RIGHT         5

#define TARGET_ARRAY_SIZE    6

#define TARGET_TOO_CLOSE     25

/*-----( Declare objects )-----*/
YD_MotorDriver1 RobotDriver(Motor1A,Motor1B,Motor2C,Motor2D); // Set pins
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // Set pins and maximum distance.
SM RoboGo(RoboStartState);//create simple statemachine

/*-----( Declare Variables )-----*/
unsigned int uS;  // Result of a ping: MicroSeconds
unsigned int cm;  // Distance calculated for ping (0 = outside set distance range)
unsigned int cm_now; // For test

int TargetArray[TARGET_ARRAY_SIZE];  // Holds the directions a Target was found in
int DirectionsToLook = 3;  // For LookAround()
int ServoDirectionData[3] = { 2500, 1600, 600};



void setup()   /******************* SETUP: RUNS ONCE *****************/
{
  Serial.begin(115200);
  delay(1000);
  Serial.println("YourDuino Robot Kit Test");
  //--NOTE: Ultrasonic Sensor and Motor Pins set to OUTPUT by their libraries
  pinMode(SERVO_PIN,OUTPUT);
  pinMode(PIN13_LED,OUTPUT);
  RobotDriver.init();

}//--(end setup )---

/************** LOOP: RUNS CONSTANTLY **************************/
void loop()
{
  EXEC(RoboGo);//run statemachine
  delay(100);
}//--(end main loop )---


/*----------------( Declare User-written Functions )---------------*/

//------( MOTOR CONTROL FUNCTIONS )----------------
void ForwardSlow()
{
  RobotDriver.Motor1Speed(SlowMoveSpeed + SlowMoveAdjust);
  RobotDriver.Motor2Speed(SlowMoveSpeed - SlowMoveAdjust);
}
/*---------------------------*/

void ForwardMedium()
{
  RobotDriver.Motor1Speed(MediumMoveSpeed + MediumMoveAdjust);
  RobotDriver.Motor2Speed(MediumMoveSpeed - MediumMoveAdjust);
}
/*---------------------------*/
void ForwardFast()
{
  RobotDriver.Motor1Speed(FastMoveSpeed + FastMoveAdjust);
  RobotDriver.Motor2Speed(FastMoveSpeed - FastMoveAdjust);
}
/*---------------------------*/
void BackwardSlow(int HowMuch)
{
  RobotDriver.Motor1Speed(- SlowMoveSpeed );
  RobotDriver.Motor2Speed(- SlowMoveSpeed );
  delay(HowMuch);
  Stop();
}
/*---------------------------*/
void BackwardMedium(int HowMuch)
{
  RobotDriver.Motor1Speed(- MediumMoveSpeed);
  RobotDriver.Motor2Speed(- MediumMoveSpeed);
  delay(HowMuch);
  Stop();
}
/*---------------------------*/
void BackwardFast(int HowMuch)
{
  RobotDriver.Motor1Speed(- FastMoveSpeed);
  RobotDriver.Motor2Speed(- FastMoveSpeed);
  delay(HowMuch);
  Stop();
}
/*---------------------------*/
void Stop()
{
  RobotDriver.Motor1Speed(0);
  RobotDriver.Motor2Speed(0);
}
/*---------------------------*/
void SpinLeft(int HowMuch)
{
  RobotDriver.Motor1Speed(  MediumMoveSpeed);
  RobotDriver.Motor2Speed(- MediumMoveSpeed);
  delay(HowMuch);
  Stop();
}
/*---------------------------*/
void SpinRight(int HowMuch)
{
  RobotDriver.Motor1Speed(- MediumMoveSpeed);
  RobotDriver.Motor2Speed(  MediumMoveSpeed);
  delay(HowMuch);
  Stop();
}
/*---------------------------*/
unsigned int PingBlink()
{
  uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
  cm = uS / US_ROUNDTRIP_CM; // Convert ping time to distance in cm 
  Serial.print("  cm = ");
  Serial.print(cm,DEC);

  if ((cm < 40) && (cm != 0))
  {
    return(cm);
  }
  else
  {
    return(100);  // No Valid Distance
  }
}// end PingBlink
/*---------------------------*/

void PointServo(int ServoAngle)
{

  for (int i=0;i<20;i++)  // Send the pulse 10 times 
  {
    digitalWrite(SERVO_PIN,HIGH);
    delayMicroseconds(ServoAngle);
    digitalWrite(SERVO_PIN,LOW);
    delay(20);
  }
}// PointServo end
/*---------------------------*/


void LookAround()  // Sets next state if Target Found
{
  for(int Direction = 0; Direction < DirectionsToLook ; Direction ++)
  {
    Serial.print("DIRECTION = ");
    Serial.print(Direction,DEC);
    PointServo(ServoDirectionData[Direction]); // Get servo pulse width from array
    delay(200);
    cm_now = PingBlink();    // Read the Ultrasonic distance

    Serial.print(" cm_now = ");
    Serial.println(cm_now,DEC);
    if (cm_now < TARGET_TOO_CLOSE) digitalWrite(PIN13_LED,HIGH);

    if (cm_now < 40)
    {
      TargetArray[TARGET_FOUND_ANY ] = true;
    }
    else TargetArray[TARGET_FOUND_ANY ] = false;

    if ((cm_now < TARGET_TOO_CLOSE) && (Direction == 2)) //LEFT
    {
      TargetArray[TARGET_LEFT ] = true;
      Serial.println("TargetLeft");
    }

    if ((cm_now < TARGET_TOO_CLOSE) && (Direction == 1)) //Center
    {
      TargetArray[TARGET_CENTER ] = true;
      Serial.println("TargetCenter");
    }

    if ((cm_now < TARGET_TOO_CLOSE) && (Direction == 0)) //RIGHT
    {
      TargetArray[TARGET_RIGHT ] = true;
      Serial.println("TargetRight");
    }

  }// END Directions

}// END LookAround

/**************************( STATE MACHINE FUNCTIONS )******************************/
State RoboStartState()
{
  Serial.println("+++ RoboStartState");
  digitalWrite(PIN13_LED,LOW);    // LED Means Target Too Close
  ForwardFast();  // Start moving forward
  delay(25);
  ForwardSlow();  // Start moving forward
  delay(600);
  RoboGo.Set(RoboStopLookState);

}// END State RoboStartState


State RoboStopLookState()
{
  Serial.println("+++ RoboStopLookState");

  for (int i = 0; i < TARGET_ARRAY_SIZE; i++) TargetArray[i] = false;
  Stop();  // Start moving forward
  LookAround();     // Ping Ultrasonic in different directions, Set TargetArray

  if      (TargetArray[TARGET_CENTER ] == true)  RoboGo.Set(RoboTargetCenter);
  else if (TargetArray[TARGET_LEFT ]   == true)  RoboGo.Set(RoboTargetLeft);
  else if (TargetArray[TARGET_RIGHT ]  == true)  RoboGo.Set(RoboTargetRight);
  else     RoboGo.Set(RoboStartState);

}// END State RoboStartState


State RoboTargetLeft()
{
  Serial.println("***** RoboTargetLeft");
  Stop();
  BackwardSlow(500);
  SpinRight(500);
  delay(500);
  RoboGo.Set(RoboStartState);
}// END State RoboTargetLeft ----------------------

State RoboTargetCenter()
{
  Serial.println("***** RoboTargetCenter");
  Stop();
  BackwardSlow(500);
  SpinLeft(1100);
  delay(500);
  RoboGo.Set(RoboStartState);
} // END State RoboTargetCenter -----------------

State RoboTargetRight()
{
  Serial.println("***** RoboTargetRight");
  Stop();
  delay(500);
  BackwardSlow(500);
  SpinLeft(500);
  delay(500);
  RoboGo.Set(RoboStartState);
}// END State RoboTargetRight -----------------

//*********( THE END )***********


COLLISION AVOIDANCE 2 (1.20): Smoother behavior (Not Available for Version 2 Yet)

Copy and paste this Sketch into a blank Arduino IDE page, save it with a name, Verify it, and Upload it to the Robot.


zz