So I have finally found a code that seems like it is going to work from this node:
https://www.robotshop.com/letsmakerobots/node/2744
However, I want to use a sharp IR sensor instead of the ping, as the sharp is cheaper and im on a strict budget for this project. I have no idea how to change the code, as I am just beginning to learn the language. Can anyone help me out? Here is the source code from that node:
/*
Rover - 1.0
Buhatkj - Oct/2008
*/
#include <Servo.h>
//mimimum safe distance in front of us in inches
int minSafeDist = 7;
//PING and pan servo head
unsigned long echo = 0;
int ultraSoundSignal = 12; // Ultrasound signal pin
unsigned long ultrasoundValue = 0;
int servoPin = 10;
Servo servo1;
int SERVOCENTER = 97;
int SERVOLEFT = 67;
int SERVORIGHT = 127;
//motor 1 = right
int m1a = 4;
int m1b = 3;
int m1e = 2;
//motor 2 = left
int m2a = 7;
int m2b = 6;
int m2e = 5;
//pan ping distance vars all in inches
int leftDist = 0;
int rightDist = 0;
int centerDist = 0;
//for debugging
int ledPin = 13; // LED connected to digital pin 13
//this is the cheapest, crappiest circular buffer evar
int actionBuffer[10];
void initBuffer(){
for(int i = 0; i < 10; i++){
actionBuffer[i] = 0;
}
}
void pushBuffer(int x){
for(int i = 0; i < 9; i++){
actionBuffer[i+1] = actionBuffer[i];
}
actionBuffer[0] = x;
}
int oscillating(){
int osc = 0;
for(int i = 0; i < 5; i++){
if ((actionBuffer[i] == 1)||(actionBuffer[i] == 2)){
osc++;
}
}
return osc;
}
void setup() // run once, when the sketch starts
{
//init all pins
pinMode(ledPin, OUTPUT); // sets the digital pin as output
pinMode(ultraSoundSignal,OUTPUT);
pinMode(servoPin, OUTPUT); // sets the digital pin as output
servo1.attach(servoPin);
servo1.write(97); //center our servo
pinMode(m1a, OUTPUT); // sets the digital pin as output
pinMode(m1b, OUTPUT); // sets the digital pin as output
pinMode(m1e, OUTPUT); // sets the digital pin as output
pinMode(m2a, OUTPUT); // sets the digital pin as output
pinMode(m2b, OUTPUT); // sets the digital pin as output
pinMode(m2e, OUTPUT); // sets the digital pin as output
//enable the motors
digitalWrite(m1e, HIGH); // on
digitalWrite(m2e, HIGH); // on
initBuffer();
Serial.begin(9600);
Serial.println("starting...");
//SelfTest();
//Serial.println("done self test...");
}
unsigned long ping(){
pinMode(ultraSoundSignal, OUTPUT); // Switch signalpin to output
digitalWrite(ultraSoundSignal, LOW); // Send low pulse
delayMicroseconds(2); // Wait for 2 microseconds
digitalWrite(ultraSoundSignal, HIGH); // Send high pulse
delayMicroseconds(5); // Wait for 5 microseconds
digitalWrite(ultraSoundSignal, LOW); // Holdoff
pinMode(ultraSoundSignal, INPUT); // Switch signalpin to input
digitalWrite(ultraSoundSignal, HIGH); // Turn on pullup resistor
echo = pulseIn(ultraSoundSignal, HIGH); //Listen for echo
ultrasoundValue = (echo / 58.138) * .39; //convert to CM then to inches
Serial.println(ultrasoundValue);
return ultrasoundValue;
}
void AllStop(){
digitalWrite(m2e, LOW); // off
digitalWrite(m1e, LOW); // off
}
void AllForward(){
digitalWrite(m2e, HIGH);
digitalWrite(m2a, HIGH);
digitalWrite(m2b, LOW);
digitalWrite(m1e, HIGH);
digitalWrite(m1a, HIGH);
digitalWrite(m1b, LOW);
}
void AllReverse(){
digitalWrite(m2e, HIGH);
digitalWrite(m2a, LOW);
digitalWrite(m2b, HIGH);
digitalWrite(m1e, HIGH);
digitalWrite(m1a, LOW);
digitalWrite(m1b, HIGH);
}
void SpinRight(){
digitalWrite(m2e, HIGH);
digitalWrite(m2a, LOW);
digitalWrite(m2b, HIGH);
digitalWrite(m1e, HIGH);
digitalWrite(m1a, HIGH);
digitalWrite(m1b, LOW);
}
void SpinLeft(){
digitalWrite(m2e, HIGH);
digitalWrite(m2a, HIGH);
digitalWrite(m2b, LOW);
digitalWrite(m1e, HIGH);
digitalWrite(m1a, LOW);
digitalWrite(m1b, HIGH);
}
void LookAround(){
//look around
servo1.write(SERVOLEFT); //left servo
delay(175); // waits for a second
leftDist = ping();
servo1.write(SERVOCENTER); //center our servo
delay(175); // waits for a second
centerDist = ping();
servo1.write(SERVORIGHT); //right servo
delay(175); // waits for a second
rightDist = ping();
servo1.write(SERVOCENTER); //center our servo
delay(175); // waits for a second
Serial.println(leftDist);
Serial.println(centerDist);
Serial.println(rightDist);
}
void LookAhead(){
//servo1.write(SERVOCENTER); //center our servo
//delay(175); // waits for a second
centerDist = ping();
}
void loop() // run over and over again
{
digitalWrite(ledPin, HIGH); // sets the LED on
LookAhead();
//LookAround();
if(centerDist >= minSafeDist){
AllForward();
pushBuffer(0);
delay(100);
}else{
//stop
AllStop();
delay(100);
//have a look around
LookAround();
if((oscillating() > 2)||(rightDist == leftDist)){
//best to turn around
SpinLeft();
initBuffer();
pushBuffer(3);
delay(2000);
//go in the direction that read farthest
}else if(leftDist > rightDist){
//best to turn left
SpinLeft();
pushBuffer(1);
delay(500);
}else if(rightDist >leftDist){
//best to turn right
SpinRight();
pushBuffer(2);
delay(500);
}else{
//this should NEVER happen
//best to turn around
SpinLeft();
initBuffer();
pushBuffer(3);
delay(2000);
}
}
delay(100);
digitalWrite(ledPin, LOW); // sets the LED off
}
Any help would be appreciated. Thanks!
