#include <SoftwareSerial.h>
#include <Servo.h>

#define rxPin 2
#define txPin 3
SoftwareSerial altSerial(rxPin, txPin);
Servo myservo;  // create servo object to control a servo

int pos = 0;      // variable to store the servo position
const byte numChars = 32;
char receivedChars[numChars];  // an array to store the received data
boolean newData = false;
unsigned int oldAlt = 9999;
unsigned int newAlt = 0;
unsigned long launchTime;
char rc;

void setup() {
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  altSerial.begin(9600);
  Serial.begin(9600);
  Serial.println("<Arduino is ready>");
}

void loop() {
  recvWithEndMarker();
  showNewData();
  if (newAlt == 505) {
    Serial.println("alt = 505");
    Brake();
  }
}

void recvWithEndMarker() {
  static byte ndx = 0;
  char endMarker = '\n';

  while (altSerial.available() > 0 && newData == false) {
    Serial.println("servo on");
    rc = altSerial.read();

    if (rc != endMarker) {
      receivedChars[ndx] = rc;
      ndx++;
      if (ndx >= numChars) {
        ndx = numChars - 1;
      }
    } else {
      receivedChars[ndx] = '\0'; // terminate the string
      ndx = 0;
      newData = true;
    }
  }
}

void showNewData() {
  if (newData == true) {
    //Serial.print("Altitude = ");
    //Serial.println(receivedChars);
    newAlt = atoi(receivedChars);
    Serial.println(newAlt);
    newData = false;
  }
}

void Brake() {
  for (pos = 0; pos <= 180; pos += 1) {
    // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo.write(pos);
    delay(500);  // waits 15 ms for the servo to reach the position
  }
  for (pos = 90; pos >= 0; pos -= 1) {
    // goes from 180 degrees to 0 degrees
    myservo.write(pos);
    delay(500);  // waits 15 ms for the servo to reach the position
  }
  delay(5000);
}