Robot Code Examples

More examples will be added during the next couple weeks...

>> Jump to: Obstacle Course Robot
You may need ONE or MORE of the following Arduino Libraries in order to compile the code.

Obstacle Course RobotBack to Top

Navigate the Maze and head straight to the finish line.


#include <Servo.h>
#include <NewPing.h>

#define STOP 1
#define FORWARD 2
#define LEFT 3
#define RIGHT 4

int TriggerPin = 2;
int EchoPin =  3;
int LeftServoPin = 9;
int RightServoPin = 10;
int LEDPin = 13;

int LeftServoStop = 1550;
int RightServoStop = 1520;

int WallTriggerDistance = 20;

NewPing sonar(TriggerPin, EchoPin, 500);
Servo LeftServo;
Servo RightServo;

void setup()
{
  Serial.begin(9600);
  LeftServo.attach(LeftServoPin);
  RightServo.attach(RightServoPin);
  pinMode(LEDPin, OUTPUT);
  
  RunObstacleCourse();
}

void loop() {
  //DistanceToWall();
  //delay(100);  
}

void RunObstacleCourse() {

  while(DistanceToWall() > WallTriggerDistance) {
    Move(FORWARD);
  }
  
  Move(LEFT);
  
  while(DistanceToWall() > WallTriggerDistance) {
    Move(FORWARD);
  }

  Move(LEFT);
  
  while(DistanceToWall() > WallTriggerDistance) {
    Move(FORWARD);
  }

  Move(RIGHT);
  
  while(DistanceToWall() > WallTriggerDistance) {
    Move(FORWARD);
  }

  Move(RIGHT);
  
  while(DistanceToWall() > WallTriggerDistance) {
    Move(FORWARD);
  }
  
  Move(STOP);
}

int DistanceToWall() {
  int Distance = sonar.ping_cm(); //get the data in centimeters
  delay(50);
  Serial.print("Ping: ");
  Serial.print(Distance);
  Serial.println(" cm");
  if(Distance < WallTriggerDistance) {
    digitalWrite(LEDPin, HIGH);
  }
  else {
    digitalWrite(LEDPin, LOW);
  }
  return(Distance);
}

void Move(int Direction) {
  switch(Direction) {
    case FORWARD:
      LeftServo.writeMicroseconds(1000);
      RightServo.writeMicroseconds(2000);
      Serial.println("Move Forward");
      delay(500);
      //Move(STOP);
      break;
    case LEFT:
      LeftServo.writeMicroseconds(1000);
      RightServo.writeMicroseconds(1000);
      Serial.println("Move Left");
      delay(2000);
      break;
    case RIGHT:
      LeftServo.writeMicroseconds(2000);
      RightServo.writeMicroseconds(2000);
      Serial.println("Move Right");
      delay(2000);
      break;
    case STOP:
      LeftServo.writeMicroseconds(LeftServoStop);
      RightServo.writeMicroseconds(RightServoStop);
      Serial.println("Move Stop");
      delay(500);
      break;          
    default:
      LeftServo.writeMicroseconds(LeftServoStop);
      RightServo.writeMicroseconds(RightServoStop);
      Serial.println("Move Default");
      delay(500);
    break;
  }
  
}