// avoidSim
// - need to fix the motors
// - need to replace delay() with event timing
bool DEBUG = 1; // 1 = Serial Monitor debug prints; 0 = NO debug prints
#include <Servo.h> //Servo motor library - for lookLeft and lookRight
Servo lookServo; // Create a Servo "look" object to control
Servo steerServo; // Create a Servo "steer" object to control
#define lookServoPin 11
#define steerServoPin 10
boolean goesForward = false;
boolean movefwd;
#define HOME 90 // servo home
#include <NewPing.h> //Ultrasonic sensor function library. You must install this library
#define trig_pin A1 //analog input 1
#define echo_pin A2 //analog input 2
#define maximum_distance 200
int distance = 0;
NewPing sonar(trig_pin, echo_pin, maximum_distance); // Create a NewPing object to control
// DC Motor: = L298N requires:
// LeftForward = IN1 HIGH, IN2 GND
// LeftReverse = IN1 GND , IN2 HIGH
// RightForward = IN3 HIGH, IN4 GND
// RightReverse = IN3 GND, IN4 HIGH
// RIGHT DC motor with L298N H-bridge control pins
// const int LeftMotorEnable = 3; // ENA or PWM of RIGHT motor
const int RightMotorForward = 4;
const int RightMotorBackward = 5;
// LEFT DC motor with L298N H-bridge control pins
// const int LeftMotorEnable = 8; // ENA or PWM of LEFT motor
const int LeftMotorForward = 7;
const int LeftMotorBackward = 6;
// Head lights, tail lights, reverse lights, directional lights
#include <Adafruit_NeoPixel.h>
#define pixPin 2
int pixNum = 32;
Adafruit_NeoPixel pixels (pixNum, pixPin, NEO_GRB + NEO_KHZ800);
int16_t x = 0, y = 0; // counters
void setup() {
Serial.begin(115200);
randomSeed(analogRead(0)); // random seed to create random "distance" measurements
pixels.begin(); // lights
// pinMode(LeftMotorEnable, OUTPUT); // pin 8 -
pinMode(LeftMotorForward, OUTPUT); // pin 7
pinMode(LeftMotorBackward, OUTPUT); // pin 6
pinMode(RightMotorBackward, OUTPUT); // pin 5
pinMode(RightMotorForward, OUTPUT); // pin 4
// pinMode(RightMotorEnable, OUTPUT); // pin 3 -
lookServo.attach(lookServoPin); // servo pin
steerServo.attach(steerServoPin); // steer pin
lookServo.write(HOME); // go to HOME
steerServo.write(HOME); // go to HOME
}
void loop() {
int distanceRight = 0;
int distanceLeft = 0;
distance = readPing();
// delay(50);
if (distance <= 20) {
if (DEBUG)
Serial.print("\nObject:");
if (DEBUG)
if (distance < 10)
Serial.print(" ");
if (DEBUG)
Serial.print(distance);
if (DEBUG)
Serial.print("cm ");
moveStop();
moveBackward();
moveStop();
distanceLeft = lookLeft();
if (DEBUG) {
distanceLeft = random(30);
Serial.print("Object:");
if (distanceLeft < 10) Serial.print(" ");
Serial.print(distanceLeft);
Serial.print("cm");
}
distanceRight = lookRight();
if (DEBUG) {
distanceRight = random(30);
Serial.print("Object:");
if (distanceRight < 10) Serial.print(" ");
Serial.print(distanceRight);
Serial.print("cm");
}
if (distanceRight >= distanceLeft) {
turnRight();
moveStop();
} else {
turnLeft();
moveStop();
}
} else {
moveForward();
}
}
int lookRight() {
if (DEBUG)
Serial.print(" lookRIGHT ");
lookServo.write(180); // look right
delay(500); // wait for device to catch up
distance = readPing(); // distance is global
lookServo.write(HOME); // home
return distance;
}
int lookLeft() {
if (DEBUG)
Serial.print(" lookLEFT ");
lookServo.write(0); // look left
delay(500); // wait for device to catch up
distance = readPing();
lookServo.write(HOME); // homne
return distance;
}
int readPing() {
int cm = sonar.ping_cm();
if (cm == 0) {
cm = 250; // ?? will this not make the robot roll forward?
}
return cm;
}
void moveStop() {
movefwd = 0;
if (DEBUG)
Serial.print("moveSTOP");
steerServo.write(HOME); // steer left
digitalWrite(RightMotorForward, LOW);
digitalWrite(LeftMotorForward, LOW);
digitalWrite(RightMotorBackward, LOW);
digitalWrite(LeftMotorBackward, LOW);
pixelBrakes();
delay(500);
}
void moveForward() {
if (DEBUG) {
if (!movefwd) {
movefwd = 1;
Serial.print("\nmoveFORWARD ");
pixelForward();
}
// these counters are for a cleaner serial monitor
x++;
if (x > 10) {
Serial.print("> ");
x = 0;
y++;
if (y == 45) {
y = 0;
movefwd = 0;
}
}
}
steerServo.write(HOME); // steer forward
digitalWrite(LeftMotorForward, HIGH);
digitalWrite(RightMotorForward, HIGH);
digitalWrite(LeftMotorBackward, LOW);
digitalWrite(RightMotorBackward, LOW);
}
void moveBackward() {
if (DEBUG)
Serial.print(" moveBACK ");
digitalWrite(LeftMotorBackward, HIGH);
digitalWrite(RightMotorBackward, HIGH);
digitalWrite(LeftMotorForward, LOW);
digitalWrite(RightMotorForward, LOW);
pixelReverse();
delay(500);
}
void turnRight() {
if (DEBUG)
Serial.print(" turnRIGHT ");
steerServo.write(135); // steer right
// delay(500);
pixelRight();
digitalWrite(LeftMotorForward, HIGH);
digitalWrite(RightMotorBackward, HIGH);
digitalWrite(LeftMotorBackward, LOW);
digitalWrite(RightMotorForward, LOW);
delay(500);
}
void turnLeft() {
if (DEBUG)
Serial.print(" turnLEFT ");
steerServo.write(45); // steer left
// delay(500);
pixelLeft();
digitalWrite(LeftMotorBackward, HIGH);
digitalWrite(RightMotorForward, HIGH);
digitalWrite(LeftMotorForward, LOW);
digitalWrite(RightMotorBackward, LOW);
delay(500);
}
void pixelForward() {
for (int i = 0; i < 8; i++) {
pixels.setPixelColor (i + 8, pixels.Color(255, 255, 255));
pixels.setPixelColor (i + 16, pixels.Color(255, 255, 255));
}
for (int i = 0; i < 8; i++) {
pixels.setPixelColor (i + 00, pixels.Color(191, 0, 0));
pixels.setPixelColor (i + 24, pixels.Color(191, 0, 0));
}
pixels.show();
pixels.show();
}
void pixelBrakes() {
pixels.clear();
pixels.show();
for (int i = 0; i < 8; i++) {
pixels.setPixelColor (i + 00, pixels.Color(255, 0, 0));
pixels.setPixelColor (i + 24, pixels.Color(255, 0, 0));
}
pixels.show();
delay(500);
pixels.clear();
pixels.show();
}
void pixelReverse() {
for (int i = 0; i < 8; i++) {
pixels.setPixelColor (i + 0, pixels.Color(255, 255, 255));
pixels.setPixelColor (i + 24, pixels.Color(255, 255, 255));
}
pixels.show();
delay(500);
pixels.clear();
pixels.show();
}
void pixelRight() {
for (int j = 0; j < 3; j++) { // blink three times
pixels.clear();
pixels.show();
delay(200);
for (int i = 0; i < 8; i++) {
pixels.setPixelColor (i + 0, pixels.Color(255, 191, 000));
pixels.setPixelColor (i + 8, pixels.Color(255, 191, 000));
}
pixels.show();
delay(200);
}
delay(200);
pixels.clear();
pixels.show();
}
void pixelLeft() {
for (int j = 0; j < 3; j++) { // blink three times
pixels.clear();
pixels.show();
delay(200);
for (int i = 0; i < 8; i++) {
pixels.setPixelColor (i + 16, pixels.Color(255, 191, 000));
pixels.setPixelColor (i + 24, pixels.Color(255, 191, 000));
}
pixels.show();
delay(200);
}
delay(200);
pixels.clear();
pixels.show();
}LOOK
STEER