Pictures:
Code: (Still working on the comments)
Code: Select all
/*
Robotin: This code will let Robotin search for obstacles in the way,
and if any found withing 7 inches it will look to its right and to its
left and turn to the one with the biggest available space
Written by: SuperMiguel
Date: Feb-02-2010
*/
#include <ServoTimer1.h> // Enables the servo library
#include <AFMotor.h> // Enables the Motor library
ServoTimer1 servo1; // Tells that the servo is connected to the servo port 1
AF_DCMotor motor1(1); // Motor 1 is connected to the port 1 on the motor shield
AF_DCMotor motor2(2); // Motor 2 is connected to the port 2 on the motor shield
int minSafeDist = 11 ; //Minimum distance
int pingPin = 2; // Ping sensor is connected to digital port # 2
int centerDist, leftDist, rightDist; // Define variables center, left and right dist
long duration, inches, cm; // Define variables for Ping sesor
void setup() {
servo1.attach(10); // Servo is attached to pin 10 in the motor shield
servo1.write(80); // Center the Ping sesor puts in in 80 degrees
motor1.setSpeed(215); // Sets the speed of the first motor
motor2.setSpeed(200); // Sets the speed of the second motor
Serial.begin(9600); // Enables Serial monitor for debugging purposes
Serial.println("Serial test!"); // Test the Serial comunication
}
void AllStop() {
motor1.run(RELEASE); // Turns off motor 1
motor2.run(RELEASE); // Turns off motor 2
}
void AllForward() { // Makes the robot go forward
motor1.run(FORWARD); // Motor 1 goes forward
motor2.run(FORWARD); // Motor 2 goes forward
Serial.println("Going forward"); // Prints a line in the serial monitor
}
void Rotate(){ // Makes the robot rotate
motor1.run(BACKWARD); // Motor 1 goes forward
motor2.run(FORWARD); // Motor 2 goes backward
delay(1600); // Time required to rotate
Serial.println("Rotating"); // Prints a line in the serial monitor
}
void turnRight() {
motor2.run(RELEASE); // turns off motor 2
motor1.run(FORWARD); // Motor 1 goes forward
delay(1500);
Serial.println("Motors going Right");
}
void turnLeft() {
motor2.run(FORWARD); // Motor 2 goes forward
motor1.run(RELEASE); // turns off motor 1
delay(1500);
Serial.println("Motors going Left");
}
void loop() {
LookAhead();
Serial.print(inches);
Serial.println(" inches");
if(inches >= minSafeDist){
AllForward();
delay(100);
}else{
AllStop();
LookAround();
if(rightDist > leftDist){
turnRight();
}else{
turnLeft();
}
}
}
unsigned long ping() {
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);
cm = microsecondsToCentimeters(duration);
}
long microsecondsToInches(long microseconds)
{
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}
void LookAhead() {
servo1.write(80);
delay(175);
ping();
}
void LookAround(){
servo1.write(160);
delay(320);
ping();
leftDist = inches;
servo1.write(0);
delay(620);
ping();
rightDist = inches;
servo1.write(80);
delay(275);
Serial.print("RightDist: ");
Serial.println(rightDist);
Serial.print("LeftDist: ");
Serial.println(leftDist);
}