Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*
- Autonomous roving robot
- Parts:
- -Arduino Uno
- -Parallax PING))) ultrasonic sensor
- -Adafruit motor shield http://www.ladyada.net/make/mshield/
- -2WD Mobile Platform http://www.makershed.com/ProductDetails.asp?ProductCode=MKSEEED7
- -Lever switch (for back bumper)
- -Indicator LED
- -Energizer XPAL XP8000 battery (custom cable and self-built regulator for motors)
- Original motion functions and design from Robot Living 8/30/2010
- Code borrowed from example sketches:
- -Ping
- Original code created June 2011
- by Daniel Gentleman
- thoughtfix@gmail.com
- http://thoughtfix.com
- Additional credits:
- Victor Brilon
- Mark Balliet
- luckylarry.co.uk for the math on the Sharp sensor
- Advice from arduino.cc forum
- Advice from from #arduino on irc.freenode.net
- Advice from adafruit forums
- Further inspiration
- -Adafruit Industries
- -Make Magazine
- Adafruit Motor shield library
- Wiring (remember, analog 0 is also digital 14)
- - Left motor to Motor 1
- - Right motor to Motor 2
- - LEDs on digital 15 and 16
- - PING))) on digital 17
- - Sharp IR sensors on digital 18 and 19
- - Lever switch (bumper attached) on digital 14
- - USB power (from a USB battery) to the Arduino
- - Regulated power (I used an Energizer XPAL XP8000 regulated to +5v) for servos
- */
- #include <AFMotor.h>
- AF_DCMotor motorL(1, MOTOR12_1KHZ);
- AF_DCMotor motorR(2, MOTOR12_1KHZ);
- int BSensor = 14; //Back sensor
- int FLEDPin = 15; //Forward LED
- int BLEDPin = 16; //Backup LED
- int pingPin = 17; //Front sensor
- int FSensorR = 18; //Front right sensor
- int FSensorL = 19; //Front left sensor
- int count = 0;
- int b=0;
- int bs=0;
- int sensorValue;
- int forwardSensor;
- int sensorValueB=1;
- int leftSensor=0;
- int rightSensor=0;
- int obstacle=0;
- void setup() {
- Serial.begin(9600);
- randomSeed(analogRead(0));
- pinMode(BLEDPin, OUTPUT);
- pinMode(FLEDPin, OUTPUT);
- pinMode(BSensor, INPUT);
- pinMode(pingPin, INPUT);
- pinMode(FSensorR, INPUT);
- pinMode(FSensorL, INPUT);
- motorL.setSpeed(200);
- motorR.setSpeed(200);
- motorL.run(RELEASE);
- motorR.run(RELEASE);
- }
- void loop()
- {
- uint8_t i;
- Forward();
- }
- void Forward () // Charge forth!
- {
- digitalWrite(FLEDPin, HIGH);
- digitalWrite(BLEDPin, LOW);
- ForwardSensor ();
- if (obstacle == 0){ //unless something is in the way
- Serial.println("Going forward.");
- motorL.run(FORWARD);
- motorR.run(FORWARD);
- }
- }
- void ForwardSensor ()
- {
- leftSensor = (ForwardSensorLeft());
- rightSensor =(ForwardSensorRight());
- forwardSensor = (getDistance());
- delay(30);
- // Serial.print(leftSensor);
- // Serial.println(" left");
- // Serial.print(rightSensor);
- // Serial.println(" right");
- // Serial.print(forwardSensor);
- // Serial.println(" front");
- if (forwardSensor <=30 || leftSensor <= 30 || rightSensor <= 30 )
- { // I gave 10cm of "wiggle room" so it doesn't turn endlessly.
- obstacle=1;
- if (ForwardSensorRight() > (ForwardSensorLeft()-10)){
- RightBackward();
- }
- else if (ForwardSensorRight() < (ForwardSensorLeft()-10)){
- LeftBackward();
- }
- else{
- Backward();
- }
- }
- else {
- obstacle = 0;
- }
- sensorValue=0;
- rightSensor=0;
- leftSensor=0;
- }
- int ForwardSensorRight ()
- { // This function is here so you don't have to re-write code
- // if you use a different sensor.
- sensorValue = irDistance(FSensorR);
- return sensorValue;
- }
- int ForwardSensorLeft ()
- { // This function is here so you don't have to re-write code
- // if you use a different sensor.
- sensorValue = irDistance(FSensorL);
- return sensorValue;
- }
- void BackwardSensor ()
- {
- sensorValueB = digitalRead(BSensor);
- if (sensorValueB == 1)
- {
- Serial.println("Object detected while going backwards.");
- motorL.run(RELEASE);
- motorR.run(RELEASE);
- for (bs=0; bs <= 3; bs++) // Add blinky lights for personality on detecting object when backing up.
- {
- digitalWrite(FLEDPin, HIGH);
- digitalWrite(BLEDPin, HIGH);
- delay(5);
- digitalWrite(FLEDPin, LOW);
- digitalWrite(BLEDPin, LOW);
- delay (5);
- }
- b=31;
- bs=0;
- Forward();
- }
- sensorValueB = 0;
- }
- void Backward ()
- {
- motorL.run(RELEASE);
- motorR.run(RELEASE);
- digitalWrite(FLEDPin, LOW);
- digitalWrite(BLEDPin, HIGH);
- Serial.println("Going backwards.");
- do
- {
- b++;
- motorL.run(BACKWARD);
- motorR.run(BACKWARD);
- delay(20);
- BackwardSensor ();
- }
- while (b < 20);
- b=0;
- }
- void LeftBackward ()
- {
- motorL.run(RELEASE);
- motorR.run(RELEASE);
- digitalWrite(FLEDPin, LOW);
- digitalWrite(BLEDPin, HIGH);
- Serial.println("Going left backwards.");
- do
- {
- b++;
- // Serial.print("b = ");
- // Serial.print(b);
- motorL.run(BACKWARD);
- motorR.run(RELEASE);
- BackwardSensor ();
- delay (20);
- }
- while (b < 20);
- b=0;
- }
- void RightBackward ()
- {
- motorL.run(RELEASE);
- motorR.run(RELEASE);
- digitalWrite(FLEDPin, LOW);
- digitalWrite(BLEDPin, HIGH);
- Serial.println("Going right backwards.");
- do
- {
- b++;
- // Serial.print("b = ");
- // Serial.print(b);
- motorL.run(RELEASE);
- motorR.run(BACKWARD);
- BackwardSensor ();
- delay (20);
- }
- while (b < 20);
- b=0;
- }
- int getDistance()
- {
- // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
- // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
- reread: // takes another reading if cm=0
- long duration, cm;
- pinMode(pingPin, OUTPUT);
- digitalWrite(pingPin, LOW);
- delayMicroseconds(2);
- digitalWrite(pingPin, HIGH);
- delayMicroseconds(5);
- digitalWrite(pingPin, LOW);
- // The same pin is used to read the signal from the PING))): a HIGH
- // pulse whose duration is the time (in microseconds) from the sending
- // of the ping to the reception of its echo off of an object.
- pinMode(pingPin, INPUT);
- duration = pulseIn(pingPin, HIGH);
- // convert the time into a distance
- cm = microsecondsToCentimeters(duration);
- // Serial.print(cm);
- // Serial.print("cm");
- // Serial.println();
- delay(100);
- // if (cm == 0) {
- // goto reread;
- // }
- return cm;
- }
- long microsecondsToCentimeters(long microseconds)
- {
- // The speed of sound is 340 m/s or 29 microseconds per centimeter.
- // The ping travels out and back, so to find the distance of the
- // object we take half of the distance travelled.
- return (microseconds/58);
- }
- int irDistance(int irPin) {
- float volts = analogRead(irPin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
- int distance = 32.5*pow(volts, -1.10); // theretical distance 32.5/ (1/Volts)S
- // Serial.print(distance);
- // Serial.print(" from ");
- // Serial.println(irPin);
- delay (10);
- return distance; // print the raw analog value to serial port
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement