*/
//Software Serial vars
#include <SoftwareSerial.h>
SoftwareSerial mySerial(0, 10);//rx tx
//Servo vars
#include <Servo.h>
Servo myservo;
int pos = 35;
int i = 0;
long loopit = 0;
//Ultrasonic vars
#define BackTrigPin 2
#define BackEchoPin 3
#define FrontTrigPin 4
#define FrontEchoPin 5
long FrontDuration,
BackDuration,
FrontDistance,
BackDistance,
LastFront,
LastBack,
PreFrontDistance,
PreBackDistance;
//Switch vars
#define BeerPin 7
int state = HIGH; // the current state of the output pin
int reading; // the current reading from the input pin
int previous = HIGH; // the previous reading from the input pin
long time = 0; // the last time the output pin was toggled
long debounce = 200;
//Setup
void setup() {
//Sensors
pinMode(BackTrigPin, OUTPUT);
pinMode(BackEchoPin, INPUT);
pinMode(FrontTrigPin, OUTPUT);
pinMode(FrontEchoPin, INPUT);
//Servo
myservo.attach(13);
//Switch
pinMode(BeerPin, INPUT);
//Comm
mySerial.begin(9600);
Serial.begin (4800);
delay(3000);
}
//Loop
void loop() {
Serial.println("Loop " + loopit);
if(loopit == 0) {
exitcooler();
}
// while(FrontDistance > 0) {
wander();
checkbeer();
wall();
checkbeer();
loopit++;
// }
}
//Front Ultrasonic Sensor Function
//Uses debounce and a smoothing method that stores previous value.
//This previous value is used in a conditional to prevent any unwelcome data spikes.
void frontsensor() {
Serial.println("Front Sensor");
digitalWrite(FrontTrigPin, LOW); // Added this line
delayMicroseconds(2); // Added this line
digitalWrite(FrontTrigPin, HIGH);
delayMicroseconds(10); // Added this line
digitalWrite(FrontTrigPin, LOW);
FrontDuration = pulseIn(FrontEchoPin, HIGH);
PreFrontDistance = (FrontDuration/2) / 29.1;
if(loopit !=0) {
LastFront = FrontDistance;
}
if(loopit == 0) {
LastFront = PreFrontDistance;
}
if(LastFront-PreFrontDistance > 10 || LastFront-PreFrontDistance < -10) {
FrontDistance = LastFront;
}
if(LastFront-PreFrontDistance > 10 || LastFront-PreFrontDistance < -10) {
FrontDistance = PreFrontDistance;
//stop();
//lookstraight();
}
Serial.print(FrontDistance);
Serial.println(" cm - Front sensor");
delay(200);
}
//Back Ultrasonic Sensor Function
//Same as front sensor
void backsensor() {
Serial.println("Back Sensor");
digitalWrite(BackTrigPin, LOW); // Added this line
delayMicroseconds(2); // Added this line
digitalWrite(BackTrigPin, HIGH);
delayMicroseconds(10); // Added this line
digitalWrite(BackTrigPin, LOW);
BackDuration = pulseIn(BackEchoPin, HIGH);
PreBackDistance = (BackDuration/2) / 29.1;
if(loopit !=0) {
LastBack = BackDistance;
}
if(loopit == 0) {
LastBack = PreBackDistance;
}
if(LastBack-PreBackDistance > 10 || LastBack-PreBackDistance < -10) {
BackDistance = LastBack;
}
if(LastBack-PreBackDistance > 10 || LastBack-PreBackDistance < -10) {
BackDistance = PreBackDistance;
}
// Serial.print(BackDistance);
// Serial.println(" cm - Back sensor");
delay(200);
}
//Forward Function
//Passes a byte to the Ardy/Seeed motor shield that tells it to spin the back wheels forward
void goforward() {
Serial.println("Go Forward");
mySerial.print("^");
Serial.println("^");
}
//Forward Function
//Passes a byte to the Ardy/Seeed motor shield that tells it to spin the back wheels backward
void goback() {
Serial.println("Go Back");
mySerial.print("R");
Serial.println("R");
}
//Forward Right Function
//Passes a byte to the Ardy/Seeed motor shield that tells it to spin the back wheels forward and turn the front wheels to the right
void goforwardright() {
Serial.println("Go Forward Right");
mySerial.print("1");
Serial.println("1");
}
//Forward Left Function
//Passes a byte to the Ardy/Seeed motor shield that tells it to spin the back wheels forward and turn the front wheels to the left
void goforwardleft() {
Serial.println("Go Forward Left");
mySerial.print("2");
Serial.println("2");
}
//Back Right Function
//Passes a byte to the Ardy/Seeed motor shield that tells it to spin the back wheels backward and turn the front wheels to the right
void gobackright() {
Serial.println("Go Back Right");
mySerial.print("3");
Serial.println("3");
}
//Back Left Function
//Passes a byte to the Ardy/Seeed motor shield that tells it to spin the back wheels backward and turn the front wheels to the left
void gobackleft() {
Serial.println("Go Back Left");
mySerial.print("4");
Serial.println("4");
}
//Stop Function
//Tells the Ardy motorshield to stop the wheels
void stop() {
Serial.println("Stop");
mySerial.print("S");
Serial.println("S");
}
void lookstraight() {
Serial.println("Look Straight");
pos = 35;
myservo.write(pos);
frontsensor();
backsensor();
}
void lookslightleft() {
Serial.println("Look Slight Left");
pos = 35+30;
myservo.write(pos);
frontsensor();
backsensor();
}
void lookslightright() {
Serial.println("Look Slight Right");
pos = 35-30;
myservo.write(pos);
frontsensor();
backsensor();
}
void lookperp() {
Serial.println("Look Perp");
pos = 35+90;
myservo.write(pos);
debounce;
frontsensor();
backsensor();
}
void exitcooler() {
Serial.println("Exit Cooler");
lookstraight();
while(FrontDistance < 60 && FrontDistance != 0) {
goback();
}
lookperp();
if(FrontDistance > BackDistance) {
gobackright();
}
if(BackDistance <= FrontDistance) {
gobackleft();
}
}
void wander() {
Serial.println("Wander");
while(FrontDistance > 60) {
lookstraight();
goforward();
}
}
void wall() {
Serial.println("Wall");
stop();
lookstraight();
if(FrontDistance < 60) {
lookslightright();
if(FrontDistance < 60) {
lookslightleft();
if(FrontDistance < 60) {
lookperp();
}
}
}
}
void checkbeer() {
Serial.println("Check Beer");
reading = digitalRead(BeerPin);
time = millis();
previous = reading;
debounce;
reading = digitalRead(BeerPin);
if (reading == HIGH && previous == HIGH && millis() - time > debounce) {
time = millis();
}
if (reading == LOW && previous == LOW && millis() - time > debounce) {
time = millis();
returntocooler();
}
}
void returntocooler() {
Serial.println("Return to Cooler");
}
/*
int pinI1=8;//define I1 interface
int pinI2=11;//define I2 interface
int speedpinA=9;//enable motor A
int pinI3=12;//define I3 interface
int pinI4=13;//define I4 interface
int speedpinB=10;//enable motor B
int motorspeed =1023;//define the motorspeed of motor
void setup()
{
mySerial.begin(9600);
Serial.begin(4800);
pinMode(pinI1,OUTPUT);
pinMode(pinI2,OUTPUT);
pinMode(speedpinA,OUTPUT);
pinMode(pinI3,OUTPUT);
pinMode(pinI4,OUTPUT);
pinMode(speedpinB,OUTPUT);
}
void forward()
{
analogWrite(speedpinA,motorspeed);//input a simulation value to set the speed
analogWrite(speedpinB,motorspeed);
digitalWrite(pinI4,HIGH);//turn DC Motor B move clockwise
digitalWrite(pinI3,LOW);
digitalWrite(pinI2,LOW);//turn DC Motor A move anticlockwise
digitalWrite(pinI1,HIGH);
}
void backward()//
{
analogWrite(speedpinA,motorspeed);//input a simulation value to set the speed
analogWrite(speedpinB,motorspeed);
digitalWrite(pinI4,LOW);//turn DC Motor B move anticlockwise
digitalWrite(pinI3,HIGH);
digitalWrite(pinI2,HIGH);//turn DC Motor A move clockwise
digitalWrite(pinI1,LOW);
}
void left()//
{
analogWrite(speedpinA,motorspeed);//input a simulation value to set the speed
analogWrite(speedpinB,motorspeed);
digitalWrite(pinI4,HIGH);//turn DC Motor B move clockwise
digitalWrite(pinI3,LOW);
digitalWrite(pinI2,HIGH);//turn DC Motor A move clockwise
digitalWrite(pinI1,LOW);
}
void right()//
{
analogWrite(speedpinA,motorspeed);//input a simulation value to set the speed
analogWrite(speedpinB,motorspeed);
digitalWrite(pinI4,LOW);//turn DC Motor B move anticlockwise
digitalWrite(pinI3,HIGH);
digitalWrite(pinI2,LOW);//turn DC Motor A move clockwise
digitalWrite(pinI1,HIGH);
}
void stop()//
{
digitalWrite(speedpinA,LOW);// Unenble the pin, to stop the motor. this should be done to avid damaging the motor.
digitalWrite(speedpinB,LOW);
delay(500);
}
void loop()
{
char msg = char(mySerial.read());
Serial.println(msg);
if(msg == '<') {
left();
}
if(msg == '>') {
right();
}
if(msg == '^') {
forward();
}
if(msg == 'R') {
backward();
}
if(msg == 'S') {
stop();
}
delay(500);
}

