Search Algorithms-3 With Motors
// SearchAlgorithm with line follower
// this code is starter code for doing a search
// it uses a servo with an Ultrasonic distance sensor mounted on it
//
// modify it to learn basic coding by creating print
// statements, variables, functions and loops
#include <Servo.h> // servo library from Arduino
Servo testServo; // define Servo name
#define SERVO_PIN 3
#define SPEAKER_PIN 12
#include <NewPing.h>
#define TRIGGER_PIN 4 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 5 // 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.
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
int distance;
int servoAngle;
boolean turningright;
boolean found;
boolean linefollow;
/*************Declarations, and Variables ****************/
// for reflectance sensors
#define rightSensorPin A0
#define middleSensorPin A1
#define leftSensorPin A2
#define speakerPin 12
#define BLACKlimit 500 // typical range white > 100, black <750
#define WHITElimit 100
int rightSensor;
int middleSensor;
int leftSensor;
// for motors and motor controller
// Left motor pins
#define enableL 6 // ENA speed pin for left motor
#define LPin1 7 // IN1 on Motor controller board
#define LPin2 8 // IN2
// Right motor pins
#define enableR 11 // ENB speed pin for right motor
#define RPin1 9 // IN3 on Motor controller board
#define RPin2 10 // IN4
int rightspeed = 140; // change these to balance motors to go straight
int leftspeed = 140;
boolean notOnLine = true;
#include <Adafruit_NeoPixel.h>
#include <avr/power.h>
#define PIN 4
#define NUMPIXELS 6
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);
void setup()
{
Serial.begin(115200); //setup Serial Monitor
Serial.println("LineFollow code starting…");
testServo.attach(SERVO_PIN); // set up servos on digital pins 8
pinMode(SPEAKER_PIN, OUTPUT);
// set all the motor control pins to outputs
pinMode(enableR, OUTPUT);
pinMode(enableL, OUTPUT);
pinMode(RPin1, OUTPUT);
pinMode(RPin2, OUTPUT);
pinMode(LPin1, OUTPUT);
pinMode(LPin2, OUTPUT);
pixels.begin(); // This initializes the NeoPixel library.
turningright = true;
servoAngle = 0;
linefollow = true;
found = false;
} //end of setup
void loop()
{
/************************** Read in data ***********/
// read the input on analog pin 0, 1, 2:
readSensors();
/************************** Line-Following Logic ***********/
if (middleSensor > BLACKlimit)
// Centered on line: left = BLACK, middle = BLACK, right = BLACK
{
Serial.println(" FORWARD");
forward();
}
else if ((leftSensor > BLACKlimit) && (rightSensor < BLACKlimit))
// too far right
{
Serial.println("BEAR LEFT");
motorsOff();
twistLeft();
}
else if ((leftSensor < BLACKlimit) && (rightSensor > BLACKlimit))
// too far left
{
Serial.println(" BEAR RIGHT");
motorsOff();
twistRight();
}
// else if (middleSensor < BLACKlimit) //off the tape more than a little
//last case - too far off line, stop and do a search
{
// Serial.println(" STOPPING");
// motorsOff();
// notOnLine = true; // set boolean value for search function
// search();
}
delay(50);
if (!found)
{
// Move servo to next position
Serial.print("servoAngle = "); Serial.println(servoAngle);
testServo.write(servoAngle);
// delay (20);
if (turningright) servoAngle++;
else servoAngle --;
if (servoAngle == 180) turningright = false;
if (servoAngle == 0) turningright = true;
// get data from sensors
readDistanceSensor();
Serial.println(distance);
// logic when something is found
if ((10 < distance) && (distance < 30)) // any delay in this loop will slow down search
{
beep();
beep();
beep();
found = true;
}
}
} // end of loop()
void readDistanceSensor()
{
distance = sonar.ping_cm(); // Send ping, get ping time in microseconds (uS).
Serial.print("Ping: ");
Serial.print (distance); // print result
Serial.println("cm");
}
void beep()
{
Serial.println("beep played");
digitalWrite(SPEAKER_PIN, HIGH);
delay(75);
digitalWrite(SPEAKER_PIN, LOW);
}
// this code is starter code for doing a search
// it uses a servo with an Ultrasonic distance sensor mounted on it
//
// modify it to learn basic coding by creating print
// statements, variables, functions and loops
#include <Servo.h> // servo library from Arduino
Servo testServo; // define Servo name
#define SERVO_PIN 3
#define SPEAKER_PIN 12
#include <NewPing.h>
#define TRIGGER_PIN 4 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 5 // 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.
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
int distance;
int servoAngle;
boolean turningright;
boolean found;
boolean linefollow;
/*************Declarations, and Variables ****************/
// for reflectance sensors
#define rightSensorPin A0
#define middleSensorPin A1
#define leftSensorPin A2
#define speakerPin 12
#define BLACKlimit 500 // typical range white > 100, black <750
#define WHITElimit 100
int rightSensor;
int middleSensor;
int leftSensor;
// for motors and motor controller
// Left motor pins
#define enableL 6 // ENA speed pin for left motor
#define LPin1 7 // IN1 on Motor controller board
#define LPin2 8 // IN2
// Right motor pins
#define enableR 11 // ENB speed pin for right motor
#define RPin1 9 // IN3 on Motor controller board
#define RPin2 10 // IN4
int rightspeed = 140; // change these to balance motors to go straight
int leftspeed = 140;
boolean notOnLine = true;
#include <Adafruit_NeoPixel.h>
#include <avr/power.h>
#define PIN 4
#define NUMPIXELS 6
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);
void setup()
{
Serial.begin(115200); //setup Serial Monitor
Serial.println("LineFollow code starting…");
testServo.attach(SERVO_PIN); // set up servos on digital pins 8
pinMode(SPEAKER_PIN, OUTPUT);
// set all the motor control pins to outputs
pinMode(enableR, OUTPUT);
pinMode(enableL, OUTPUT);
pinMode(RPin1, OUTPUT);
pinMode(RPin2, OUTPUT);
pinMode(LPin1, OUTPUT);
pinMode(LPin2, OUTPUT);
pixels.begin(); // This initializes the NeoPixel library.
turningright = true;
servoAngle = 0;
linefollow = true;
found = false;
} //end of setup
void loop()
{
/************************** Read in data ***********/
// read the input on analog pin 0, 1, 2:
readSensors();
/************************** Line-Following Logic ***********/
if (middleSensor > BLACKlimit)
// Centered on line: left = BLACK, middle = BLACK, right = BLACK
{
Serial.println(" FORWARD");
forward();
}
else if ((leftSensor > BLACKlimit) && (rightSensor < BLACKlimit))
// too far right
{
Serial.println("BEAR LEFT");
motorsOff();
twistLeft();
}
else if ((leftSensor < BLACKlimit) && (rightSensor > BLACKlimit))
// too far left
{
Serial.println(" BEAR RIGHT");
motorsOff();
twistRight();
}
// else if (middleSensor < BLACKlimit) //off the tape more than a little
//last case - too far off line, stop and do a search
{
// Serial.println(" STOPPING");
// motorsOff();
// notOnLine = true; // set boolean value for search function
// search();
}
delay(50);
if (!found)
{
// Move servo to next position
Serial.print("servoAngle = "); Serial.println(servoAngle);
testServo.write(servoAngle);
// delay (20);
if (turningright) servoAngle++;
else servoAngle --;
if (servoAngle == 180) turningright = false;
if (servoAngle == 0) turningright = true;
// get data from sensors
readDistanceSensor();
Serial.println(distance);
// logic when something is found
if ((10 < distance) && (distance < 30)) // any delay in this loop will slow down search
{
beep();
beep();
beep();
found = true;
}
}
} // end of loop()
void readDistanceSensor()
{
distance = sonar.ping_cm(); // Send ping, get ping time in microseconds (uS).
Serial.print("Ping: ");
Serial.print (distance); // print result
Serial.println("cm");
}
void beep()
{
Serial.println("beep played");
digitalWrite(SPEAKER_PIN, HIGH);
delay(75);
digitalWrite(SPEAKER_PIN, LOW);
}