YourDuino-Basic-Robot-Kit-Software

From ArduinoInfo
Jump to navigation Jump to search

YourDuino-Basic-Robot-Kit-Software (Original Version 1)


NOTE: This software is for the original version of the Yourduino Basic Robot kit.

Do NOT use the software on this page for the current versions of the YourDuino Robot Kit, or if you are making a robot from the current How-To pages.


If you have the 2015 and later Version GO HERE:

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. [/YourDuino-Basic-Robot-Kit-Software#robogo1 See the software sketches HERE:] NOTE: You need the software libraries downloaded and installed (see below).

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)
  • Servo
  • Ultrasonic Distance Sensor
  • Motor Driver


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.0.5 which has the new easy Library Install feature.

Using LIBRARIES:

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.
  See more about Libraries HERE:
  And.. we suggest you use the new easier Library Install Method HERE:
 
  Here are the libraries we will use:
  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.

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: 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:
   - 
   - 
 - V1.00 09/11/12
   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. Then run the test sketch below.
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.

/* YourDuino Basic Robot Kit: Ultrasonic Sensor Test
 - WHAT IT DOES
 - SEE the comments after "//" on each line below
 - CONNECTIONS: 
 - Sensor Vcc to YourDuinoRobo1 RED
 - Sensor Ground to YourDuinoRobo1 BLACK
 - Sensor Trig to YourDuinoRobo1 pin 5
 - Sensor Echo to YourDuinoRobo1 pin 6   
 - V1.00 10/28/13
 Questions: terry@yourduino.com */

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

/*-----( Declare Constants and Pin Numbers )-----*/
#define TRIGGER_PIN  5  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     6  // 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 )-----*/


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.
  unsigned int 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:

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 (There is a lot of friction in a new battery case and they might not make contact). (We recommend rechargeable NiMH type batteries). 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. 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.
 
  The battery case can be held in place with a piece of the #22 blue wire, or with adhesive Velcro. Position it so the wires are on the side of the RoboRED power connector.
 
  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 Basic Robot Kit: Test Motors and Motor Driver
 - WHAT IT DOES: Tests the motor driver, battery connection and
   motors by running them each in both directions.
 - SEE the comments after "//" on each line below
 - CONNECTIONS:
   Pins 9 (Motor1 PWM) and 10 (Motor2 PWM) are predefined, unchangeable
   Motor1A  pin 2   // Motor pins to Driver board
   Motor1B  pin 4
   Motor2A  pin 7
   Motor2B  pin 8
   
 - V1.00 10/28/13
   Questions: terry@yourduino.com */

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

/*-----( Declare Constants )-----*/
// NOTE: Pins 9 (Motor1) and 10 (Motor2) are predefined, unchangeable
#define  Motor1A  2   // Motor pins to Driver board
#define  Motor1B  4
#define  Motor2A  7
#define  Motor2B  8

#define  RampDelay  10

/*-----( Declare objects )-----*/
YD_MotorDriver1 RobotDriver(Motor1A,Motor1B,Motor2A,Motor2B);

/*-----( Declare Variables )-----*/
int i ;  //For looping

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

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


void loop()   /*----( LOOP: RUNS CONSTANTLY )----*/
{

/*----( Ramp Motor1 Up and Down both directions )-------*/
  for (int i = 0; i <= 400; i++)
  {
    RobotDriver.Motor1Speed(i);
    delay(RampDelay);
  }

  for (int i = 400; i >= -400; i--)
  {
    RobotDriver.Motor1Speed(i);
    delay(RampDelay);
  }

  for (int i = -400; i <= 0; i++)
  {
    RobotDriver.Motor1Speed(i);
    delay(RampDelay);
  }



/*----( Ramp Motor2 Up and Down both directions )-------*/
  for (int i = 0; i <= 400; i++)
  {
    RobotDriver.Motor2Speed(i);
    delay(RampDelay);
  }

  for (int i = 400; i >= -400; i--)
  {
    RobotDriver.Motor2Speed(i);
    delay(RampDelay);
  }

  for (int i = -400; i <= 0; i++)
  {
    RobotDriver.Motor2Speed(i);
    delay(RampDelay);
  }

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

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

/* ( THE END ) */



AUTONOMOUS COLLISION-AVOIDING ROBOT SKETCHES:

You need the software libraries downloaded and installed (see above).
This assumes you have built the robot with:

  • YourDuinoRobo1
  • 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) 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.

COLLISION AVOIDANCE 1 (1.16): More hesitant 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.
 

/* YourDuino Basic Robot Kit Collision-avoidance Test
 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
 Motor1A  pin 2   // Motor pins to Driver board
 Motor1B  pin 4
 Motor2A  pin 7
 Motor2B  pin 8
 
 TRIGGER_PIN   5  // The Ultrasonic Sensor
 ECHO_PIN      6  
 
 SERVO_PIN    11  // The "Look Around" servo
 PIN13_LED    13  // Onboard LED. Lit when target in range
 
 - 
 - V1.16 01/28/2013  Updating motor variables
 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  2   // Motor pins to Driver board
#define  Motor1B  4
#define  Motor2A  7
#define  Motor2B  8
#define  RampDelay  10

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

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

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

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

// Ultrasonic Sensor Pins
#define TRIGGER_PIN  5  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     6  // 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

//----( "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,Motor2A,Motor2B); // 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 == 0)) //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 == 2)) //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

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

/* YourDuino Basic Robot Kit Collision-avoidance Test
 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
 Motor1A  pin 2   // Motor pins to Driver board
 Motor1B  pin 4
 Motor2A  pin 7
 Motor2B  pin 8
 
 TRIGGER_PIN   5  // The Ultrasonic Sensor
 ECHO_PIN      6  
 
 SERVO_PIN    11  // The "Look Around" servo
 PIN13_LED    13  // Onboard LED. Lit when target in range
 
 - 
 - V1.20 01/28/2013  Smoother behavior
 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  2   // Motor pins to Driver board
#define  Motor1B  4
#define  Motor2A  7
#define  Motor2B  8
#define  RampDelay  10

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

#define  VerySlowMoveSpeed     270
#define  VerySlowMoveAdjust     -4   // Adjust for straight move: - Left + Right??

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

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

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

// Ultrasonic Sensor Pins
#define TRIGGER_PIN  5  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     6  // 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

//----( "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,Motor2A,Motor2B); // 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 ForwardVerySlow()
{
  RobotDriver.Motor1Speed(VerySlowMoveSpeed + VerySlowMoveAdjust);
  RobotDriver.Motor2Speed(VerySlowMoveSpeed - VerySlowMoveAdjust);
}
/*---------------------------*/
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 == 0)) //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 == 2)) //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;
  ForwardVerySlow();
  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()  // Something on the left, so Turn Right
{
  Serial.println("***** RoboTargetLeft");
  Stop();
  BackwardSlow(500);
  SpinRight(500);
  RoboGo.Set(RoboStartState);
}// END State RoboTargetLeft ----------------------

State RoboTargetCenter() // Something ahead, so Turn Around
{
  Serial.println("***** RoboTargetCenter");
  Stop();
  BackwardSlow(500);
  SpinLeft(1400);
  RoboGo.Set(RoboStartState);
} // END State RoboTargetCenter -----------------

State RoboTargetRight() // Something on the right, so Turn Left
{
  Serial.println("***** RoboTargetRight");
  Stop();
  delay(500);
  BackwardSlow(500);
  SpinLeft(500);
  RoboGo.Set(RoboStartState);
}// END State RoboTargetRight -----------------

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



zz

Retrieved from "http://arduinoinfo.mywikis.net/w/index.php?title=YourDuino-Basic-Robot-Kit-Software&oldid=120145"