#include <Servo.h> 

/* Start-up Instructions

1) Powerup all components
2) Arm the throttle with the 'A' command (only need to do this once)
3) Throttle up to at least START_THROTTLE with the 'U' command
4) Throttle down with the 'D' command
5) Left and right L/R commands

*** For emergancies, stop the throttle with the 'S' command.

 /********************************************************************************************************
                                      Definition of Constants
********************************************************************************************************/
// Serial port commands
#define COMMAND_POLL 'P'               // Polls the Arduino for throttle and rudder positions
#define COMMAND_RUDDER_LEFT 'L'        // Move rudder left
#define COMMAND_RUDDER_RIGHT 'R'       // Move rudder right
#define COMMAND_THROTTLE_UP 'U'        // Throttle power +
#define COMMAND_THROTTLE_DOWN 'D'      // Throttle power -
#define COMMAND_THROTTLE_ARM 'A'       // Arm the throttle
#define COMMAND_THROTTLE_STOP 'S'      // Kill the throttle

// Pin references
#define rudPin 9
#define thrPin 10

// Motor min & max values; in degrees (0-180)
#define MIN_RUDDER 35     // Hard left
#define MID_RUDDER 98     // Approximate 'straight' location
#define MAX_RUDDER 160    // Hard right
#define MIN_THROTTLE 30   // Throttle will NOT spin up at this speed
#define START_THROTTLE 45 // Minimum for throttle to start
#define MAX_THROTTLE 180  // This is not defined yet


/********************************************************************************************************
                                          Global Variables
********************************************************************************************************/
Servo servoRud;
Servo servoThr;
int rudderPosition;    // variable to store the servo position 
int throttlePosition;    // variable to store the servo position 
byte incomingByte;


// Setup function for Arduino
void setup() {
  Serial.begin(57600);
  Serial.println("Arduino is initializing...");  
  servoRud.attach(rudPin);
  
  rudderPosition = MID_RUDDER;
  throttlePosition = MIN_THROTTLE;
  
  delay(15);
  Serial.println("Ready to receive commands...");
}

// Main Loop
void loop(){
    if( Serial.available() > 0 ) {
    incomingByte = 0;
    incomingByte = Serial.read(); 
    switch( incomingByte ) {
        case COMMAND_POLL:
          Serial.print("Rudder Position: "); 
          Serial.println(rudderPosition);
          Serial.print("Throttle Position: "); 
          Serial.println(throttlePosition);
          break;
        case COMMAND_RUDDER_LEFT:
          rudderLeft (5);
          Serial.print("Rudder Position: ");
          Serial.println(rudderPosition);
          break;
        case COMMAND_RUDDER_RIGHT:
          rudderRight (5);
          Serial.print("Rudder Position: ");
          Serial.println(rudderPosition);
          break;
        case COMMAND_THROTTLE_UP:
          throttleUp (5);
          Serial.print("Throttle Position: "); 
          Serial.println(throttlePosition);
          break;
        case COMMAND_THROTTLE_DOWN:
          throttleDown (5);
          Serial.print("Throttle Position: "); 
          Serial.println(throttlePosition);
          break;
        case COMMAND_THROTTLE_ARM:
          throttleArm();
          Serial.println("Throttle is ARMED... "); 
          break;
        case COMMAND_THROTTLE_STOP:
          throttleStop();
          Serial.println("Throttle is disabled..."); 
          break;
     }  
  }
    servoRud.write(rudderPosition);
    servoThr.write(throttlePosition);
  
}

/********************************************************************************************************
                                         Support Functions
********************************************************************************************************/

void rudderLeft(int amount){
    rudderPosition = rudderPosition + amount;
    if (rudderPosition > MAX_RUDDER){
      rudderPosition = MAX_RUDDER;
    }
}

void rudderRight(int amount){
    rudderPosition = rudderPosition - amount;
    if (rudderPosition < MIN_RUDDER){
      rudderPosition = MIN_RUDDER;
    }
}

void throttleUp(int amount){
    throttlePosition = throttlePosition + amount;
    if (throttlePosition > MAX_THROTTLE){
      throttlePosition = MAX_THROTTLE;
    }
}

void throttleDown(int amount){
    throttlePosition = throttlePosition - amount;
    if (throttlePosition < MIN_THROTTLE){
      throttlePosition = MIN_THROTTLE;
    }
}

void throttleArm(){
    servoThr.attach(thrPin);
    throttlePosition = MIN_THROTTLE;
}

void throttleStop(){
    throttlePosition = MIN_THROTTLE;
}

/*END OF FILE*/