What Do I Build Next? An Arduino Uno Mini Robot

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

/*
 * Ultrasonic Simple
 * Prints the distance read by an ultrasonic sensor in
 * centimeters. They are supported to four pins ultrasound
 * sensors (liek HC-SC04) and three pins (like PING)))
 * and Seeed Studio sensors).
 *
 * The circuit:
 * * Module HR-SC04 (four pins) or PING))) (and other with
 *   three pins), attached to digital pins as follows:
 * ---------------------    --------------------
 * | HC-SC04 | Arduino |    | 3 pins | Arduino |
 * ---------------------    --------------------
 * |   Vcc   |   5V    |    |   Vcc  |   5V    |
 * |   Trig  |   12    | OR |   SIG  |   13    |
 * |   Echo  |   13    |    |   Gnd  |   GND   |
 * |   Gnd   |   GND   |    --------------------
 * ---------------------
 * Note: You do not obligatorily need to use the pins defined above
 * 
 * By default, the distance returned by the read()
 * method is in centimeters. To get the distance in inches,
 * pass INC as a parameter.
 * Example: ultrasonic.read(INC)
 *
 * created 3 Apr 2014
 * by Erick Simões (github: @ErickSimoes | twitter: @AloErickSimoes)
 * modified 23 Jan 2017
 * by Erick Simões (github: @ErickSimoes | twitter: @AloErickSimoes)
 * modified 03 Mar 2017
 * by Erick Simões (github: @ErickSimoes | twitter: @AloErickSimoes)
 * modified 11 Jun 2018
 * by Erick Simões (github: @ErickSimoes | twitter: @AloErickSimoes)
 *
 * This example code is released into the MIT License.
 */

#include <Ultrasonic.h>

/*
 * Pass as a parameter the trigger and echo pin, respectively,
 * or only the signal pin (for sensors 3 pins), like:
 * Ultrasonic ultrasonic(13);
 */
Ultrasonic ultrasonic(12, 13);
int distance;

void setup() {
  Serial.begin(9600);
}

void loop() {
  // Pass INC as a parameter to get the distance in inches
  distance = ultrasonic.read();
  
  Serial.print("Distance in CM: ");
  Serial.println(distance);
  delay(1000);
}