First attempt at collision detectin/evasion

evasion_test.c

#include <AFMotor.h>
#include <Servo.h>

AF_DCMotor motorFrontLeft(3);
AF_DCMotor motorBackLeft(4);
AF_DCMotor motorFrontRight(2);
AF_DCMotor motorBackRight(1);

int servoPin = 10;
int pingPin = 15;
Servo headServo;

int speed = 128;
int collisionDistance = speed / 4;

int shortCount;

// 0 is straigh ahead. -90 is left, 90 is right
void lookAt(int degreesForward) {
  // Right: 0, Ahead 94, Left: 188
  int d = (-degreesForward + 90);
  d += d * 0.05;
  
  headServo.write(d);
}

void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Motor test!");

  headServo.attach(servoPin);
  
  lookAt(-90);
  delay(1000);
  lookAt(90);
  delay(1000);
  
  lookAt(0);
  
  
  // turn on motor
  motorFrontLeft.setSpeed(speed);
  motorBackLeft.setSpeed(speed);
  motorFrontRight.setSpeed(speed);
  motorBackRight.setSpeed(speed);
 
  motorFrontLeft.run(RELEASE);
  motorBackLeft.run(RELEASE);
  motorFrontRight.run(RELEASE);  
  motorBackRight.run(RELEASE); 
  
  shortCount = 0;
}

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 / 29 / 2;
}

long getDistanceToNearestObjectInCentimeters() {
  long duration, cm;
  
  // Setting a 2microseconds HIGH will send out a ping.
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  
  // Switch to input
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  
  cm = microsecondsToCentimeters(duration);
  
  Serial.print(cm);
  Serial.println("cm");
  
  return cm;
}

void loop() { 
  Serial.print("shortCount: ");
  Serial.println(shortCount);

  if (shortCount < 10) {
    
    // Move forward
    motorFrontLeft.run(FORWARD);
    motorBackLeft.run(FORWARD);
    motorFrontRight.run(FORWARD);
    motorBackRight.run(FORWARD);
    
    // Nearing anything?
    long cm = getDistanceToNearestObjectInCentimeters();
    if (cm > 1 && cm < collisionDistance) {
      // Count that.
      shortCount += 2;
    } else {
      shortCount -= 1;
      if (shortCount < 0) {
        shortCount = 0;
      }
    }
    
    // Carry on
    delay(50);
  } else {
    // More than 3 near objects measured
    Serial.println("Turning!");
    
    // Stop    
    motorFrontLeft.run(RELEASE);
    motorBackLeft.run(RELEASE);
    motorFrontRight.run(RELEASE);
    motorBackRight.run(RELEASE);  
    delay(500);

    // Turn a bit
    motorFrontLeft.run(FORWARD);
    motorBackLeft.run(FORWARD);
    motorFrontRight.run(BACKWARD);
    motorBackRight.run(BACKWARD);  
    delay(500);
  
    // Stop again.
    motorFrontLeft.run(RELEASE);
    motorBackLeft.run(RELEASE);
    motorFrontRight.run(RELEASE);
    motorBackRight.run(RELEASE);  
    delay(500);
    
    // Carry on 
    shortCount = 0;
  }  
 }