Scott Beasley
Published © CC BY-SA

Simple multi-mode 4wd Rover JR-001

Small four wheeled bots are fun to build and play with, but offer a new set of challenges for the beginner. With the addition of two more...

BeginnerFull instructions provided22,827
Simple multi-mode 4wd Rover JR-001

Things used in this project

Hardware components

Runt Rover Junior
×1
Arduino UNO
Arduino UNO
×1
V5 Sheild
×1
L9110
×1
S3003 Servo
×1
Rechargeable AA batteries
NiMh and charger
×6
Female/Female Jumper Wires
Female/Female Jumper Wires
×1
Male/Female Jumper Wires
Male/Female Jumper Wires
×1
6 Slot AA battery holder.
×1
Breadboard (generic)
Breadboard (generic)
×1
Slide Switch
Slide Switch
×1
9V to Barrel Jack Connector
9V to Barrel Jack Connector
×1
Ultrasonic Sensor - HC-SR04 (Generic)
Ultrasonic Sensor - HC-SR04 (Generic)
×1
HC-SR04 Holder
×1

Story

Read more

Schematics

rover4wd_bb.png

rover4wd.fzz

Code

Code snippet #1

Plain text
/*
   Arduino obstacle avoider and follower.

   Goal in life...
      Moves forward looking for obstacles to avoid or
      keep-away from them (Force)

   Written by Scott Beasley - 2015
   Free to use or modify. Enjoy.
*/

/*
   Uses the L9110S library. It works with the L9110S h-bridge. See Step 1 for the library   code.
*/

#include <L9110Driver.h>
#include <Servo.h>

// Defines for the distance reading function of the bot.
#define turn() (left_dist >= right_dist) ? go_left () : go_right ()
#define microsecondsToCentimeters(microseconds) (unsigned long)microseconds / 29.1 / 2.0
#define MIN_ACTION_DIST 20 // 20 cm
#define MIN_AWAY_DIST 20 // 20cm from bot

// Change these defines if you use different pins
#define ECHO_PIN 8 // Digital pin 2
#define TRIGGER_PIN 3 // Digital pin 3
#define SERVO_LR 4 // Pin for left/right scan servo
#define pinAIN1 5 // define I1 interface
#define pinAIN2 6 // define I2 interface
#define pinBIN1 9 // define I3 interface
#define pinBIN2 11 // define I4 interface
#define MODE_PIN 2

// Bot modes
#define MODE_AVOID 0
#define MODE_FORCE 1

// Speed defines
#define MAXFORWARDSPEED 225 // Max speed we want moving forward
#define MAXBACKWARDSPEED 225 // Max reverse speed
#define TOPSPEED 255 // Used to help turn better on carpet and rougher surfaces.

// Various time delays used for driving and servo
#define TURNDELAY 475
#define BACKUPDELAY 400
#define SERVOMOVEDELAY 200
#define SERVOSEARCHDELAY 85
#define DEBOUNCE_TM 20 // 20ms

/*
   Globals area.
*/

// Create the motor, servo objects to interface with
L9110_Motor motor_left (pinAIN1, pinAIN2); // Create Left motor object
L9110_Motor motor_right (pinBIN1, pinBIN2); // Create Right motor object
Servo sensor_servo; // Create a servo object for our distance sensor

// Global variables
byte sweep_pos = 0; // Current position of the sensor servo
byte pos_index = 90;
byte mode = MODE_AVOID;
byte mode_change = true;
unsigned long last_pin_change;
unsigned long left_dist, right_dist; // Distance measured left and right

// function to handle the mode switch with debounce
void checkMode ( )
{
  byte mode_val = digitalRead (MODE_PIN);

  if ((long)(millis ( ) - last_pin_change) >= DEBOUNCE_TM) {
     last_pin_change = millis ( );
     // Changes the Mode the bot is in.
     if (mode_val == LOW)
        mode = MODE_AVOID;

     if (mode_val == HIGH)
        mode = MODE_FORCE;

     mode_change == true;
  }
}

void setup ( )
{
   // Uncomment if you need to debug.
   //Serial.begin (9600); // Set Serial monitor at 9600 baud
   //Serial.println ("My Follow/Avoid bot is starting up!");

   // Make sure the motors are off at start
   halt ( );

   sensor_servo.attach (SERVO_LR); // Attach the servo SERVO_LR
   sensor_servo.write (90); // Set the servo to the middle (neutral) pos

   // Set modes for distance sensor pins
   pinMode (ECHO_PIN, INPUT);  // Set echo pin as input
   pinMode (TRIGGER_PIN, OUTPUT); // Set trigger pin as output
   pinMode (MODE_PIN, INPUT); // Set mode pin as input

   // Delay to give user time to make adjustments.  Remove after done.
   //delay (30000);
}

void loop ( )
{
   checkMode ( ); // Read current switch value

   // If mode has changed, re-center the sensor servo
   if (mode_change == true) {
      mode_change = false;
      pos_index = 90;
      sweep_pos = 0;
      sensor_servo.write (pos_index);
   }

   if (mode == MODE_AVOID)
      avoid ( ); // Try not to hit anything!

   if (mode == MODE_FORCE)
      force ( ); // keep-away from the hand!
}

void avoid ( )
{
   unsigned long dist_fwd;

   // Rotate the distance sensor as we drive along
   rotate_sensor ( );

   // Give the servo time to get to position to get settled
   delay (SERVOSEARCHDELAY);

   // Get a reading from the current sensor direction
   dist_fwd = ping ( );
   //Serial.print ("Distance sensor reading: ");
   //Serial.println (dist_fwd);

   // Go forward while nothing is in the distance sensors read area
   if (dist_fwd > MIN_ACTION_DIST || dist_fwd == 0)
   {
      go_forward ( );
   }
   else // There is something in the sensors read area
   {
      halt ( ); // Stop!
      go_backward ( ); // Back up a bit
      delay (BACKUPDELAY);
      halt ( ); // Stop!

      sensor_read ( ); // Read distance left and right

      turn ( ); // Turn toward the clearest path
      delay (TURNDELAY);
      halt ( );

      // Reset to mid position before moving again
      pos_index = 90;
      sweep_pos = 0;
      sensor_servo.write (pos_index);
   }
}

void force ( )
{
   unsigned long obj_dist;

   obj_dist = ping ( );
   //Serial.print ("Distance sensor reading: ");
   //Serial.println (obj_dist);

   // Go forward while nothing is in the distance sensors read area
   if (obj_dist >= (MIN_AWAY_DIST + 20)) {
      halt ( );
      go_forward ( );
   } else if (obj_dist <= (MIN_AWAY_DIST + 5)) {
      halt ( );
      go_backward ( );

      // Reset to mid position before moving again
      pos_index = 90;
      sweep_pos = 0;
      sensor_servo.write (pos_index);
   }

   delay (250);
}

// Read the sensor after we find something in the way. This helps find a new
// path
void sensor_read ( )
{
   //Serial.println ("Server at 40 deg...");
   sensor_servo.write (40);
   delay (SERVOMOVEDELAY);
   right_dist = ping ( ); //Look to the right

   //Serial.println ("Servo at 140 deg...");
   sensor_servo.write (140);
   delay (SERVOMOVEDELAY);
   left_dist = ping ( ); // Look to the left

   // Set the servo back to the center pos
   //Serial.println ("Servo at 90 deg...");
   sensor_servo.write (90);
}

// Rotate the sensor servo at 45deg increments
void rotate_sensor ( )
{
   if (sweep_pos <= 0) {
      pos_index = 45;
   }
   else if (sweep_pos >= 180) {
      pos_index = -45;
   }

   //Serial.print ("pos_index = ");
   //Serial.println (pos_index);
   sweep_pos += pos_index;
   //Serial.print ("sweep_pos = ");
   //Serial.println (sweep_pos);
   sensor_servo.write (sweep_pos);
}

// Read the HC-SR04 uSonic sensor
unsigned long ping ( )
{
   // Trigger the uSonic sensor (HC-SR04) to send out a ping
   digitalWrite (TRIGGER_PIN, LOW);
   delayMicroseconds (5);
   digitalWrite (TRIGGER_PIN, HIGH);
   delayMicroseconds (10);
   digitalWrite (TRIGGER_PIN, LOW);

   // Measure how long the ping took and convert to cm's
   return (microsecondsToCentimeters (pulseIn (ECHO_PIN, HIGH)));
}

void go_forward ( )
{
   //Serial.println ("Going forward...");

   // Ramp the motors up to the speed.
   // Help with spining out on some surfaces and ware and tare on the GM's
   ramp_it_up (MAXFORWARDSPEED, FORWARD, BACKWARD);

   // Set to all of the set speed just incase the ramp last vat was not all of
   // it.
   motor_left.setSpeed (MAXFORWARDSPEED);
   motor_right.setSpeed (MAXFORWARDSPEED);
   motor_left.run (FORWARD|RELEASE);
   motor_right.run (BACKWARD|RELEASE);
}

void go_backward ( )
{
   //Serial.println ("Going backward...");

   // Ramp the motors up to the speed.
   // Help with spining out on some surfaces and ware and tare on the GM's
   ramp_it_up (MAXBACKWARDSPEED, BACKWARD, FORWARD);

   // Set to all of the set speed just incase the ramp last vat was not all of
   // it.
   motor_left.setSpeed (MAXBACKWARDSPEED);
   motor_right.setSpeed (MAXBACKWARDSPEED);
   motor_left.run (BACKWARD|RELEASE);
   motor_right.run (FORWARD|RELEASE);
}

void go_left ( )
{
   //Serial.println ("Going left...");

   // Ramp the motors up to the speed.
   // Help with spining out on some surfaces and ware and tare on the GM's
   ramp_it_up (TOPSPEED, BACKWARD, BACKWARD);

   // Set to all of the set speed just incase the ramp last vat was not all of
   // it.
   motor_left.setSpeed (TOPSPEED);
   motor_right.setSpeed (TOPSPEED);
   motor_left.run (BACKWARD|RELEASE);
   motor_right.run (BACKWARD|RELEASE);
}

void go_right ( )
{
   //Serial.println ("Going right...");

   // Ramp the motors up to the speed.
   // Help with spining out on some surfaces and ware and tare on the GM's
   ramp_it_up (TOPSPEED, FORWARD, FORWARD);

   // Set to all of the set speed just incase the ramp last vat was not all of
   // it.
   motor_left.setSpeed (TOPSPEED);
   motor_right.setSpeed (TOPSPEED);
   motor_left.run (FORWARD|RELEASE);
   motor_right.run (FORWARD|RELEASE);
}

void halt ( )
{
   //Serial.println ("Halt!");
   motor_left.setSpeed (0);
   motor_right.setSpeed (0);
   motor_left.run (BRAKE);
   motor_right.run (BRAKE);
}

void ramp_it_up (uint8_t speed, uint8_t lf_dir, uint8_t rt_dir)
{
  uint8_t ramp_val = 0, step_val = 0;

  step_val = abs (speed / 4);
  for (uint8_t i = 0; i < 4; i++) {
     ramp_val += step_val;
     motor_left.setSpeed (ramp_val);
     motor_right.setSpeed (ramp_val);
     motor_left.run (lf_dir|RELEASE);
     motor_right.run (rt_dir|RELEASE);
     delay (25);
  }
}

Code snippet #2

Plain text
// From the Global section....
#define DEBOUNCE_TM 20 // 20ms
.
unsigned long last_pin_change;
.
.
.
// function to handle the mode switch with debounce
void checkMode ( )
{
  byte mode_val = digitalRead (MODE_PIN);

  if ((long)(millis ( ) - last_pin_change) >= DEBOUNCE_TM) {
     last_pin_change = millis ( );
     // Changes the Mode the bot is in.
     if (mode_val == LOW)
        mode = MODE_AVOID;

     if (mode_val == HIGH)
        mode = MODE_FORCE;

     mode_change == true;
  }
}

Code snippet #3

Plain text
void ramp_it_up (uint8_t speed, uint8_t lf_dir, uint8_t rt_dir)
{
   uint8_t ramp_val = 0, step_val = 0;
   step_val = abs (speed / 4);
   for (uint8_t i = 0; i < 4; i++) {
      ramp_val += step_val;
      motor_left.setSpeed (ramp_val);
      motor_right.setSpeed (ramp_val);
      motor_left.run (lf_dir|RELEASE);
      motor_right.run (rt_dir|RELEASE);
      delay (25);
   }
}

Credits

Scott Beasley

Scott Beasley

9 projects • 61 followers
Well odd

Comments

Related channels and tags
  • Arduino
  • Robotics