Robot – program #1

This is the first program to test our differential drive robots.

//—————————————————————-
// 1st robot program
//
// The program waits for the pushbutton to be pressed, then it
// goes into a driving loop.
//—————————————————————- 

#include <Servo.h>

//—————————————————————-
// pin definitions – this defines what pins the motors, led, and
// pushbutton are connected to.
//—————————————————————-
#define RIGHT_MOTOR_PIN 9
#define LEFT_MOTOR_PIN 10
#define LED_PIN 13
#define PUSH_BUTTON_PIN 8

//—————————————————————-
// servo function variables
//—————————————————————-
Servo leftServo;
Servo rightServo;

//—————————————————————-
// setup function called at the start of the program
//—————————————————————-
void setup()
{
// setup led on digital pin 13
pinMode(LED_PIN,OUTPUT);

// set digital pin 8 to an input to sense our pushbutton switch
pinMode(PUSH_BUTTON_PIN, INPUT);
// add an internal pullup to pin 8 by calling digitalWrite
digitalWrite(PUSH_BUTTON_PIN,HIGH);

// wait for someone to push the button, blink led while waiting
while(digitalRead(PUSH_BUTTON_PIN) == HIGH)
{
digitalWrite(LED_PIN,LOW);
delay(50);
digitalWrite(LED_PIN,HIGH);
delay(50);
}
// turn on led
digitalWrite(LED_PIN,HIGH);

// set up servo functions
rightServo.attach(RIGHT_MOTOR_PIN);
leftServo.attach(LEFT_MOTOR_PIN);
}

//—————————————————————-
// main program loop
//—————————————————————-
void loop()
{
// loop forever
while(1)
{
// stop for 2 seconds
RobotStop();
delay(2000);

// move forward for 2 seconds
RobotForward();
delay(2000);

// stop for 2 seconds
RobotStop();
delay(2000);

// turn right for 1 second
RobotRight();
delay(1000);

// stop for 2 seconds
RobotStop();
delay(2000);

// move forward for 2 seconds
RobotForward();
delay(2000);

// quick led blink
digitalWrite(LED_PIN,LOW);
delay(100);
digitalWrite(LED_PIN,HIGH);
}
}

//—————————————————————-
// these are our utility functions to set the motor pwm values
//—————————————————————-
void RobotRight()
{
leftServo.write(179);
rightServo.write(179);
}

void RobotLeft()
{
leftServo.write(0);
rightServo.write(0);
}

void RobotForward()
{
leftServo.write(179);
rightServo.write(0);
}

void RobotReverse()
{
leftServo.write(0);
rightServo.write(179);
}

void RobotStop()
{
leftServo.write(90);
rightServo.write(90);
}

.