YourDuino-Basic-Robot-Kit-Software

From ArduinoInfo
Jump to: navigation, 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)</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.0.5 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.


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. </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: 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</span> (see above).</span>
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