We have a little bit of work to do here... Sharp IR sensor output can be a little inconsistent from time to time.
Let's start by installing the Arduino SharpIR library or SharpIR by guillaume-rico. And then... check out the great article on linearizing Sharp distance sensor data.
Here's a sample of how to drive the Sandblaster based on data from either the Sharp GP2Y0A02YK0F or GP2Y0A21YK distance sensor.
Servo and sensor connections are documented in-line.
#include <Adafruit_SoftServo.h>
#include <SharpIR.h>
#define ir A0
#define model 1080
Adafruit_SoftServo servo_rght; // right wheel
Adafruit_SoftServo servo_left; // left wheel
Adafruit_SoftServo servo_strr; // steering
SharpIR sharp(ir, 30, 50, model);
unsigned long tme = 0; // the last time we processed a distance check
unsigned long slc = 250; // milliseconds between distance checks
unsigned long ms = 0; // a millis() time-slice
int dis = 0; // last measured distance
int thr = 8; // threshold to trigger reverse
void setup() {
servo_rght.attach(7); // D7
servo_left.attach(6); // D6
servo_strr.attach(5); // D5
pinMode (ir, INPUT); // A0
}
void loop() {
ms = millis();
// distance samples every 250ms
if ( tme + slc < ms ) {
tme = ms;
dis = sharp.distance();
}
// course correction!
if (dis < thr) {
// right or left... pick one and step back
int lor = random(30, 100);
for (int i = 0; i < 120; i++) {
if (lor % 2 == 1) {
servo_strr.write(40);
} else {
servo_strr.write(140);
}
servo_strr.refresh();
servo_rght.write(140);
servo_rght.refresh();
servo_left.write(40);
servo_left.refresh();
delay(15);
}
} else {
// full steam ahead! Although... you could mix a distance
// calculation into this and steer around obstacles. Just
// a thought.
servo_strr.write(90);
servo_strr.refresh();
servo_rght.write(40);
servo_rght.refresh();
servo_left.write(140);
servo_left.refresh();
delay(15);
}
}
Have fun!
