RobotKitMenu-8-C

From ArduinoInfo
Jump to navigation Jump to search
print this page
[/RobotKitMenu-C BACK to MENU]

NOTE!! PROBLEM with compiling solved with new software version (below)

PROBLEMS: Email terry@yourduino.com

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 and the motor tests ran OK 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 2 (2.20): Stops to look around; 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 V2 Collision-avoidance Test 
   (Rewritten using "Switch" statements to create the State Machine ) 
   This version is slow, waits to look around.
   KIT here: http://yourduino.com/sunshop2/index.php?l=product_detail&p=400
   HOW-TO here: https://arduinoinfo.mywikis.net/wiki/RobotKitMenu
  - WHAT IT DOES:
  - Runs the robot motors
  - Looks Around with the Servo and Ultrasonic Sensor
  - Avoids obstacles
  - 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. See:
       https://arduinoinfo.mywikis.net/wiki/RobotKitMenu-6 
  REQUIRES: YD_MotorDriver1 Library
  - 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.20 02/02/2016
  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

/*-----( 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      0  // Adjust for straight move: - Left + Right??

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

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

// 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
                         // Add a Beeper on pin 13 if you want

//----( "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  // cm

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

/*-----( 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};

/*-----( Define the 'States' the robot can be in )-----*/
enum {  START_STATE,
        STOP_LOOK_STATE,
        TARGET_CENTER_STATE,
        TARGET_LEFT_STATE,
        TARGET_RIGHT_STATE
     } RobotState = START_STATE;

void setup ()
{
  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();
}

void loop()
{
/*-----( Control the Robot by changing the "State" it is in. )-----*/
  switch (RobotState)
  {
    case START_STATE: // Start here, return here if no obstacles
      Serial.println("+++ RoboStartState");
      digitalWrite(PIN13_LED, LOW);   // LED Means Target Too Close. Turn it off for now.
      ForwardFast();  // Get a good start moving forward
      delay(25);
      ForwardSlow();  // Continue moving forward
      delay(600);
      RobotState = STOP_LOOK_STATE ;  // Now change to state to look for obstacles
      break;

    case STOP_LOOK_STATE:  // Look around for obstacles; take different actions if found
      Serial.println("+++ RoboStopLookState");
      Stop();  // Stop moving forward

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

      if      (TargetArray[TARGET_CENTER ] == true)  RobotState = TARGET_CENTER_STATE;
      else if (TargetArray[TARGET_LEFT ]   == true)  RobotState = TARGET_LEFT_STATE;
      else if (TargetArray[TARGET_RIGHT ]  == true)  RobotState = TARGET_RIGHT_STATE;
      else     RobotState = START_STATE;
      break;

    case TARGET_CENTER_STATE:  // Obstacle straight ahead; back up then turn around
      Serial.println("***** RoboTargetCenter");
      Stop();
      BackwardSlow(500);
      SpinLeft(1100);  // try for 180 degrees turn
      delay(500);
      RobotState = START_STATE;
      break;

    case TARGET_LEFT_STATE:      // Obstacle on left; back up then turn right
      Serial.println("***** RoboTargetLeft");
      Stop();
      BackwardSlow(500);
      SpinRight(500);
      delay(500);
      RobotState = START_STATE;
      break;

    case TARGET_RIGHT_STATE:     // Obstacle on right; back up then turn left
      Serial.println("***** RoboTargetRight");
      Stop();
      delay(500);
      BackwardSlow(500);
      SpinLeft(500);
      delay(500);
      RobotState = START_STATE;
      break;
  }  // end of switch


} // end of 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

//****** THE END ************



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. ELSE 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.

You can 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.

Other possible modifications if you dig into the code:

  • Look at the code for avoiding objects. Add an LED and 220 ohm resistor to (available) pin 4 or pin 12 and made them be turn signals. Or maybe add LEDs and resistors where the Beeper is connected as Backup lights.
  • Modify the directions the servo points the ultrasonic and 'scan' for an object and stop in front of it. Or follow it.
  • Add new hardware to make a line follower robot. You'll probably have to use the pins the servo and ultrasonic use now.
  • Other Ideas?? email terry@yourduino.com



RobotOnBox.jpg


Go To the Next Section: [/RobotKitMenu-9-C Adding Capabilities]
OR back to the [/RobotKitMenu-C#SBS Main Menu] :

Please email comments, critiques, suggestions and questions to:
  terry@yourduino.com
Retrieved from "http://arduinoinfo.mywikis.net/w/index.php?title=RobotKitMenu-8-C&oldid=119934"