Skip to content

Instantly share code, notes, and snippets.

@thiyagaraj
Created October 17, 2012 03:53

Revisions

  1. thiyagaraj created this gist Oct 17, 2012.
    116 changes: 116 additions & 0 deletions grover.c
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,116 @@
    #include <Servo.h>
    #define HEAD_SERVO_PIN 13

    #define ULTRASONIC_TRIG_PIN 2
    #define ULTRASONIC_ECHO_PIN 5
    #define ULTRASONIC_POWER_PIN 3
    #define OBSTACLE_MIN_DISTANCE 25 //in centimeter

    #define MOVE_LEFT_PIN 12 //left
    #define MOVE_RIGHT_PIN 11 //right
    #define MOVE_FORWARD_PIN 10 //forward
    #define MOVE_BACKWARD_PIN 9 // reverse

    Servo groverHeadServo;

    void setup()
    {
    pinMode(HEAD_SERVO_PIN, OUTPUT);
    groverHeadServo.attach(HEAD_SERVO_PIN);

    // Motor pins
    pinMode(MOVE_LEFT_PIN, OUTPUT);
    pinMode(MOVE_RIGHT_PIN, OUTPUT);
    pinMode(MOVE_FORWARD_PIN, OUTPUT);
    pinMode(MOVE_BACKWARD_PIN, OUTPUT);

    groverHeadServo.write(30);
    delay(1000);
    groverHeadServo.write(130);
    delay(1000);
    groverHeadServo.write(90);
    delay(500);
    Serial.begin (9600);

    pinMode(ULTRASONIC_TRIG_PIN, OUTPUT);
    pinMode(ULTRASONIC_POWER_PIN, OUTPUT);
    pinMode(ULTRASONIC_ECHO_PIN, INPUT);
    digitalWrite(ULTRASONIC_POWER_PIN, HIGH);
    }

    void loop()
    {
    int objectInFrontStatus = 0;
    while(objectInFrontStatus == 0){
    Serial.println("Inside while loop");
    //Keep moving forward
    objectInFrontStatus = objectInFront();
    Serial.print("Status: ");
    Serial.println(objectInFrontStatus);
    digitalWrite(MOVE_FORWARD_PIN, HIGH);
    //delay(3000);
    }

    Serial.println("Outside while loop");
    digitalWrite(MOVE_FORWARD_PIN, LOW);
    //delay(5000);
    digitalWrite(MOVE_RIGHT_PIN, HIGH);
    //delay(1000);
    digitalWrite(MOVE_BACKWARD_PIN, HIGH);
    delay(1000);
    digitalWrite(MOVE_RIGHT_PIN, LOW);
    digitalWrite(MOVE_BACKWARD_PIN, LOW);
    digitalWrite(MOVE_LEFT_PIN, HIGH);
    digitalWrite(MOVE_FORWARD_PIN, HIGH);
    delay(1000);
    digitalWrite(MOVE_LEFT_PIN, LOW);

    //delay(1000);
    Serial.println("End of loop");
    /* static int val = 90;
    int angleMoveVal = 20;
    int delayVal = 4000;
    groverHeadServo.write(val);
    delay(delayVal);
    while(val <=159){
    groverHeadServo.write(val+=angleMoveVal);
    delay(delayVal);
    }
    val = 90;
    groverHeadServo.write(val);
    while(val > 0){
    groverHeadServo.write(val-=angleMoveVal);
    delay(delayVal);
    }*/
    }

    int objectInFront(){
    Serial.println("In object in front::: ");
    long duration, distance;
    digitalWrite(ULTRASONIC_TRIG_PIN, LOW);
    delayMicroseconds(2);

    digitalWrite(ULTRASONIC_TRIG_PIN, HIGH);
    delayMicroseconds(10);

    digitalWrite(ULTRASONIC_TRIG_PIN, LOW);
    duration = pulseIn(ULTRASONIC_ECHO_PIN, HIGH);

    distance = (duration/2) / 29.1;
    if (distance >= 200 || distance <= 0){
    Serial.println("Returning false - dist >=200 or <=0");
    return 0;
    }
    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");
    // delay(3000);
    if(distance < OBSTACLE_MIN_DISTANCE) {
    Serial.println("Returning true - object in front");
    return 1;
    }
    else {
    Serial.println("Returning false - object in front");
    return 0;
    }
    }