Robotica » BoBeBo (aka Insetto Robot)

/* 

    Progetto : Insetto Robot - HackLab Terni

    Il robot controlla la presenza di ostacoli, se ci sono cambia direzione, altrimenti procede in avanti.

    Luglio 2012

    Modificato: Ottobre 2012

    Autore : Cristina Begliomini

*/

#include <Servo.h> 

Servo frontServo;  

Servo rearServo;  

/* Servo motors - global variables */

int centerPos = 90; 

int frontRightUp = 72;

int frontLeftUp = 108;

int backRightForward = 75;

int backLeftForward = 105;

int walkSpeed = 150; // How long to wait between steps in milliseconds

int centerTurnPos = 96;

int frontTurnRightUp = 81;

int frontTurnLeftUp = 99;

int backTurnRightForward = 81;

int backTurnLeftForward = 111;

/* Misurazione della distanza - global variables */

int pingPin = 4;

long int duration;

long distanceFront=0; //cm

int startAvoidanceDistance=20; //distanza massima in cm per considerare un ostacolo

long microsecondsToCentimeters(long microseconds)

{

  return microseconds / 29 / 2;

}

long distanceCm(){

  pinMode(pingPin, OUTPUT);

  digitalWrite(pingPin, LOW);

  delayMicroseconds(2);

  digitalWrite(pingPin, HIGH);

  delayMicroseconds(5);

  digitalWrite(pingPin, LOW);

  pinMode(pingPin, INPUT);

  duration = pulseIn(pingPin, HIGH);

  return microsecondsToCentimeters(duration);

}

void moveForward()

{

  frontServo.write(frontRightUp);

  rearServo.write(centerPos);

  delay(65);

  rearServo.write(backLeftForward);

  delay(125);

  frontServo.write(centerPos);

  delay(65);

  frontServo.write(frontLeftUp);

  rearServo.write(centerPos);

  delay(65);

  rearServo.write(backRightForward);

  delay(125);

  frontServo.write(centerPos);

  delay(65);

}

void moveBackRight()

{

  frontServo.write(frontRightUp);

  rearServo.write(backRightForward-6);

  delay(125);

  frontServo.write(centerPos);

  rearServo.write(centerPos-6);

  delay(65);

  frontServo.write(frontLeftUp+9);

  rearServo.write(backLeftForward-6);

  delay(125);

  frontServo.write(centerPos);

  rearServo.write(centerPos);

  delay(65);

}

void moveTurnLeft()

{

  frontServo.write(frontTurnRightUp);

  rearServo.write(centerTurnPos);

  delay(65);

  rearServo.write(backTurnLeftForward);

  delay(125);

  frontServo.write(centerTurnPos);

  delay(65);

  frontServo.write(frontTurnLeftUp);

  rearServo.write(centerTurnPos);

  delay(65);

  rearServo.write(backTurnRightForward);

  delay(125);

  frontServo.write(centerTurnPos);

  delay(65);

}

void setup()

{

  frontServo.attach(2);

  rearServo.attach(3);

  pinMode(pingPin, OUTPUT);

}  

void loop()

{

  distanceFront=distanceCm();

  if (distanceFront > 1){ // Filters out any stray 0.00 error readings

    if (distanceFront<startAvoidanceDistance) {

      for(int i=0; i<=8; i++) {

        moveBackRight();

        delay(walkSpeed);

      }

      for(int i=0; i<=10; i++) {

        distanceFront=distanceCm(); // inseramo un controllo della presenza di ostacoli anche in fase di cambio di direzione

          if (distanceFront > 1){ // Filters out any stray 0.00 error readings

            if (distanceFront<startAvoidanceDistance) {

              moveBackRight();

              delay(walkSpeed);

          } else {

            moveTurnLeft();

      }

      }

      }

    } else {

      moveForward(); 

    } 

}

}