//CAMP Dare to Dream - Cougar Mountain Challenge

//Robot Basic Timed Motion
//Names: First Last
//Team: Section #, Team A##
//Date: Day, Month, Year

#include <Servo.h> // Include servo library

//multiple servo attachments causes undefined behavior.

Servo servoLeft; // Declare Left servo
Servo servoRight; // Declare Right servo

void setup() // Built in initialization block
{
   servoRight.attach(30); // Attach right signal to pin 30
   servoLeft.attach(31); // Attach left signal to pin 31

  //Set both servos to go forward at 100% speed
   rightForward( 100 );
   leftForward( 100 );

  //Wait for 1 second (The delay function uses milliseconds as input)
   delay(5000);

  //Stop both servos
   rightForward( 0 );
   leftForward( 0 );
}

void loop()
{
   //Do nothing in this loop
}





/////////
// This section combines programming tools to simplify the commands 
// needed to control servo rotation and the overall motion of the robot. 
/////////
void leftForward( int powerLevel )
{
   //Limit numbers to machine constraints
   if( 100 < powerLevel )
   { powerLevel = 100;  }
   servoLeft.writeMicroseconds(1500 + powerLevel); // Left Forward
}

void leftBackward( int powerLevel )
{
   //Limit numbers to machine constraints
   if( 100 < powerLevel )
   { powerLevel = 100;  }
   servoLeft.writeMicroseconds(1500 - powerLevel); // Left Backward
}

void rightForward( int powerLevel )
{
   //Limit numbers to machine constraints
   if( 100 < powerLevel )
   { powerLevel = 100;  }
   servoRight.writeMicroseconds(1500 - powerLevel); // Right Forward
}

void rightBackward( int powerLevel )
{
   //Limit numbers to machine constraints
   if( 100 < powerLevel )
   { powerLevel = 100;  }
   servoRight.writeMicroseconds(1500 + powerLevel); // Right Backward
}