{\rtf1\ansi\ansicpg1252\cocoartf1265\cocoasubrtf210
{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
{\colortbl;\red255\green255\blue255;}
\paperw11900\paperh16840\margl1440\margr1440\vieww10800\viewh8400\viewkind0
\pard\tx566\tx1133\tx1700\tx2267\tx2834\tx3401\tx3968\tx4535\tx5102\tx5669\tx6236\tx6803\pardirnatural

\f0\fs24 \cf0 // Caterpillar inspired in Oddbots caterpillar\
#include <Servo.h>\
#include <NewPing.h>\
#define USTrigger 11\
#define USEcho 12\
#define MaxDistance 100\
\
Servo Servo1;\
Servo Servo2;\
Servo Servo3;\
Servo Servo4;\
Servo Servo5;\
Servo Servo6;\
Servo Servo7;\
Servo Servo8;\
\
unsigned int duration;\
unsigned int distance;\
unsigned int FrontDistance;\
unsigned int LeftDistance;\
unsigned int RightDistance;\
unsigned int Time;\
unsigned int CollisionCounter;\
\
NewPing sonar(USTrigger, USEcho, MaxDistance);\
\
void setup()\
\{\
 // assign servos to pins and center servos\
Servo1.attach(2);\
Servo1.write(90);\
Servo2.attach(3);\
Servo2.write(90);\
Servo3.attach(4);\
Servo3.write(90);\
Servo4.attach(5);\
Servo4.write(90);\
Servo5.attach(6);\
Servo5.write(90);\
Servo6.attach(7);\
Servo6.write(90);\
Servo7.attach(8);\
Servo7.write(90);\
Servo8.attach(9);\
Servo8.write(90);\
delay(2000);\
\
// start serial port\
  Serial.begin(9600);\
 \
  \}\
  \
void scan()  //This function determines the distance things are away from the ultrasonic sensor\
\{\
  Serial.println("");\
  Serial.println("Scanning");\
  Time = sonar.ping(); \
  distance = Time / US_ROUNDTRIP_CM;\
  delay(500);\
\}\
\
void navigate()\
\{\
    Serial.println("There's an obstacle!");\
    Servo1.write(140);                                 //Move the servo to the left (my little servos didn't like going to 180 so I played around with the value until it worked nicely)\
    delay(1000);                                       //Wait half a second for the servo to get there\
    scan();                                           //Go to the scan function\
    LeftDistance = distance;                          //Set the variable LeftDistance to the distance on the left\
    Serial.println("Left distance = ");\
    Serial.print(distance);\
    Servo1.write(50);                                   //Move the servo to the right\
    delay(1000);                                       //Wait half a second for the servo to get there\
    scan();                                           //Go to the scan function\
    RightDistance = distance;                         //Set the variable RightDistance to the distance on the right\
    Serial.println("Right distance = ");\
    Serial.print(distance);\
    if(abs(RightDistance - LeftDistance) < 5)\
    \{\
      moveBackward();                                  //Go to the moveBackward function\
      delay(500);                                      //Pause the program for 200 milliseconds to let the robot reverse\
      moveRight();                                     //Go to the moveRight function\
      delay(500);                                      //Pause the program for 200 milliseconds to let the robot turn right\
    \}\
    else if(RightDistance < LeftDistance)                  //If the distance on the right is less than that on the left then...\
    \{\
     moveLeft();                                      //Go to the moveLeft function\
     delay(500);                                      //Pause the program for half a second to let the robot turn\
    \}\
    else if(LeftDistance < RightDistance)             //Else if the distance on the left is less than that on the right then...\
    \{\
     moveRight();                                     //Go to the moveRight function\
     delay(500);                                      //Pause the program for half a second to let the robot turn\
    \}\
\}\
\
void loop()\
\{\
  Servo1.write(90); \
  delay(1000); \
  scan();                                             //Go to the scan function\
  FrontDistance = distance;                           //Set the variable FrontDistance to the value of the distance returned from the scan function\
  Serial.println("Front distance = ");\
  Serial.print(distance);\
  if(FrontDistance > 20 || FrontDistance == 0)        //If there is nothing infront of the robot within 40cm or the distance value is 0 (which for the newping libary means no ping was returned) then...\
  \{\
  \
   moveForward();                                     //Go to the moveForward function\
//  Servo5.write(20);\
//  delay(2000);\
//  Servo5.write(170);\
//  delay(2000);\
  \
\
  \}\
  else                                                //Else (if there is something infront of the robot within 40cm) then...\
  \{\
    stand();    //Go to the stand function\
    navigate();\
  \}  \
\
\}\
\
\
void moveForward()\
\{\
Servo2.write(20);\
delay(150);\
Servo4.write(170);\
delay(150);\
Servo8.write(20);\
delay(150);\
Servo5.write(170);\
delay(150);\
Servo4.write(20);\
delay(150);\
Servo5.write(20);\
delay(150);\
Servo2.write(170);\
delay(150);\
Servo8.write(170);\
delay(150);\
Servo5.write(90);\
delay(150);\
Servo8.write(90);\
delay(150);\
Servo2.write(90);\
delay(150);\
Servo4.write(90);\
delay(150);\
\}\
\
void moveBackward()\
\{\
Servo2.write(40);\
Servo4.write(40);\
Servo5.write(140);\
Servo8.write(140);\
delay(50);\
Servo8.write(40);\
delay(100);\
Servo4.write(140);\
delay(50);\
Servo2.write(90);\
Servo8.write(90);\
Servo4.write(90);\
Servo5.write(90);\
delay(50);\
\}\
\
void moveRight()\
\{\
\
\
\
\}\
\
void moveLeft()\
\{\
\
\
\
\}\
\
void stand()\
\{\
Servo1.write(90);\
delay(50);\
Servo2.write(90);\
delay(50);\
Servo3.write(90);\
delay(50);\
Servo4.write(90);\
delay(50);\
Servo5.write(90);\
delay(50);\
Servo6.write(90);\
delay(50);\
Servo7.write(90);\
delay(50);\
Servo8.write(90);\
delay(50);\
\}}