I have posted the software below....
And yes Keeva is trying (poorly) to get out of the boxes. It was not originally intended for that purpose but it was interesting watching it try to get out. I could make it much better at getting out, but i was mostly concerned with it not running into things.
Sorry the code is poorly commented...
btw this is the first thing I ever programmed with arduino, so be gently but please point out errors.
______________________________________________________________________________________
#include <SoftwareServo.h>
SoftwareServo myservo;
const int motorLEN=3;
const int motorREN=5;
const int motor1=2;
const int motor2=6;
const int buzzer=8;
const int pingPin = 9;
int motor=0;
int pos=0;
void setup() {
int x;
myservo.attach(12);
for (x=0;x<120;x++) { //center servo on start up
myservo.write(60);
delay(5);
SoftwareServo::refresh();
}
pinMode(motorLEN, OUTPUT);
pinMode(motorREN, OUTPUT);
pinMode(motor1, OUTPUT);
pinMode(motor2, OUTPUT);
digitalWrite(motor1, HIGH); //set motor direction forward
digitalWrite(motor2, HIGH); //set motor direction forward
}
void loop() {
for(pos = 0; pos < 120; pos +=30){ //sweep servo and monitor ping output
myservo.write(pos);
SoftwareServo::refresh();
delay(10);
long inch;
inch = pingDetect();
if (inch<=10) {
halt();
}
}
for(pos = 120; pos>=1; pos-=30) {
myservo.write(pos);
delay(10);
SoftwareServo::refresh();
long inch;
inch = pingDetect();
if (inch<=10) {
halt();
}
}
}
int pingDetect() {
long duration, inches, cm;
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
inches = microsecondsToInches(duration);
delay(100);
return inches;
}
long microsecondsToInches(long microseconds){
return microseconds / 74 / 2;
}
void halt() {
digitalWrite(motorLEN,LOW); //stop left motor
digitalWrite(motorREN,LOW); //stop right motor
long leftPing;
long rightPing;
int x;
int randNum;
randomSeed(analogRead(0)); // seed random number generator from the analog input. (nothing is attached to the input)
randNum= random(125,800);
for (x=0;x<120;x++) { //look left and record distance for left
myservo.write(120);
delay(5);
SoftwareServo::refresh();
}
leftPing=pingDetect();
for (x=0;x<120;x++) { //look right and record distance for right
myservo.write(1);
delay(5);
SoftwareServo::refresh();
}
rightPing=pingDetect();
for (x=0;x<120;x++) { //center servo
myservo.write(60);
delay(5);
SoftwareServo::refresh();
}
if (leftPing<rightPing){ //compare left/right values and then turn toward the greatest distance.
digitalWrite(motor2, LOW);
digitalWrite(motorREN, HIGH);
digitalWrite(motorLEN, HIGH);
delay(randNum); // allows keeva to turn for a random amount of time.(so it don't alway turn 90 degrees)
digitalWrite(motor2, HIGH);
}
else {
digitalWrite(motor1, LOW);
digitalWrite(motorREN, HIGH);
digitalWrite(motorLEN, HIGH);
delay(randNum);
digitalWrite(motor1, HIGH);
}
}