A2PI 0.5b

I’ve been spending the past few days working on a new project of mine, A2PI ( Aeturnal Autonomous Physical Interface). A2PI is an autonomous (obviously) robot that tries to avoid obstacles and navigate the big, scary world.

A2PI is controlled by an Arduino Duemilanove, based on an ATMega328 microcontroller. The system is powered by an airsoft NiCd battery pack, rated at 8.4v. It senses the world via an infrared rangefinder mounted on a basic hobby servo, swiveling back and forth like radar. In the future, it will have rotary encoders on the wheels, as well as a touch sensor or two in the front to back away from collisions. It will also be equipped with a fully automatic airsoft gun, just in case it needs to shoot something.

Parts List:

  • Arduino Duemilanove
  • Sharp GP2Y0A02YK IR Rangefinder
  • IRLZ44NPbF MOSFET
  • Tamiya Remote Control Bulldozer
  • Hi-Tec Servo 322HD
  • TI SN754410 Motor Driver

Arduino Sketch:

#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;
  }
}

One Response to “A2PI 0.5b”

Leave a Reply

XHTML: You can use these tags: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <s> <strike> <strong>