A2PI 0.5b

#include <servo .h>            // include the standard servo library
Servo scanservo;              // set a variable to map the servo
 
int IRpin = 1;                // analog pin for reading the IR sensor
int LEDpin = 13;              // digital pin for random LED
 
// motors
// motor1
int m1p1 = 2;                 // first pin of first motor
int m1p2 = 3;                 // second pin of first motor
 
// motor2
int m2p1 = 4;                 // first pin of second motor
int m2p2 = 5;                 // second pin of second motor
 
boolean goingleft = false;    // variable to store direction of rotation for IR scan
boolean scanFinished = false; // boolean for finished scan
boolean scanMap[18];          // 10 angles, T for open to travel and F for blocked
 
int curDist = 0;              // current distance
int state = 4;                // state for state machine-style control
 
/* setup the pins, servo and serial port */
void setup() {
  scanservo.attach(14);
  // initialize the serial port:
  Serial.begin(9600);
  pinMode(m1p1, OUTPUT);
  pinMode(m2p1, OUTPUT);
  pinMode(m1p2, OUTPUT);
  pinMode(m2p2, OUTPUT);
  pinMode(LEDpin, OUTPUT);
}
 
/* update the scan and process the values */
void loop() {
  scan();
  process();
}
/* stop the motors */
void mstop() {
  digitalWrite(m2p1, LOW);
  digitalWrite(m2p2, LOW);
  digitalWrite(m1p1, LOW);
  digitalWrite(m1p2, LOW);
}
/* turn right */
void turnRight() {
  mstop();
  digitalWrite(m1p2, HIGH);
  digitalWrite(m2p1, HIGH);
}
/* turn left */
void turnLeft() {
  mstop();
  digitalWrite(m1p1, HIGH);
  digitalWrite(m2p2, HIGH);
}
/* move forwards */
void forward() {
  mstop();
  digitalWrite(m1p1, HIGH);
  digitalWrite(m2p1, HIGH);
}
/* move backwards */
void reverse() {
  mstop();
  digitalWrite(m1p2, HIGH);
  digitalWrite(m2p2, HIGH);
}
/* convert analogRead into value in cm */
float cm (float i) {
  i = 5 * i / 1034;
  if (i < 0.5)
    return 0;
  float a = 0.008271;
  float b = 939.6;
  float c = -3.398;
  float d = 17.339;
  int k = ((a + b * i) / ( 1 + c * i + d * i * i ));
  if ( k > 5 )
    return k ;
  else
    return -1000;
}
/* scan and average three readings from IR rangefinder */
int irtocm() {
  float total = 0;
  for (int i = 0; i < 3; i++) {
    int v = analogRead(IRpin);
    int k = cm(v);
    if (k >= 0)
      total += k;
    else
      i--;
    delay(5);
  }
  return (int) (total / 3);
}
/* update scanned values */
void scan() {
  // rotation
  if (goingleft)
    scanservo.write(scanservo.read() - 10);
  else
    scanservo.write(scanservo.read() + 10);
  // finished 180deg. rotation
  if (scanservo.read() < = 0 || scanservo.read() >= 180) {
    goingleft = !goingleft;
    scanFinished = true;
  }
  // delay for servo travel time
  delay(10);
  // scan
  curDist = irtocm();
  // if infinite range or far away, must be clear
  if (curDist == 0 || curDist > 75)
    scanMap[scanservo.read()/10] = true;
  else // it is close, space is blocked
    scanMap[scanservo.read()/10] = false;
}
/* reset the map */
void reset() {
  // print the map
  Serial.println("MAP");
  for (int i = 0; i < 18; i++)
    if (scanMap[i]) {
      Serial.print("T ");
    }
    else
    {
      Serial.print("F ");
      led();
    }
  Serial.println(" ");
  // set everything to false
  for (int i = 0; i < 18; i++)
    scanMap[i] = false;
}
/* blink */
void led() {
  digitalWrite(LEDpin,HIGH);
  delay(10);
  digitalWrite(LEDpin,LOW);
}
/* decision logic */
void process() {
  // only change state if the scan is finished
  if (scanFinished) {
    scanFinished = false;
    // maximum number of open spaces is 9, case TFTFTFTFTFTFTFTFTF
    int start[9];
    int length[9];
    int counter = 0;
    // init the arrays
    for (int i = 0; i < 9; i++) {
      start[i] = 0;
      length[i] = 0;
    }
    // find the open spaces
    for (int i = 0; i < 18; i++) {
      if (scanMap[i]){
        if (length[counter] == 0)
          start[counter] = i;
        length[counter] += 1;
      }
      else
        counter++;
    }
    int biggestGap = 0;
    counter = 0;
    // find the biggest open space
    for (int i = 0; i < 9; i++) {
      if (length[i] > biggestGap) {
        biggestGap = length[i];
        counter = i;
      }
    }
    // convert back into angle via average
    int centerAngle = 5 * (2 * start[counter] + length[counter]);
    // if its on the right
    if (centerAngle > 110) {
      state = 2;
    }
    // if its on the left
    else if (centerAngle < 70) {
      state = 1;
    }
    // if its in front
    else {
      state = 0;
    }
    // clear the array
    reset();
  }
  // state machine
  switch(state) {
  case 0:
    forward();
    break;
  case 1:
    turnLeft();
    break;
  case 2:
    turnRight();
    break;
  case 3:
    reverse();
    break;
  case 4:
    mstop();
    break;
  }
}