// 1. Use an Accelerometer to Measure the Current Acceleration of the Sensor
// 2. Use this information to Calculate the Current Velocity 
// 3. Derive the Height of the Sensor from the Acceleration and Velocity Readings
// 4. Open A Servo when the sensor is at its highest point or Some Time has Passed. 
// 5. Use LED's as status Markers 

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_FeatherOLED.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <ESP32Servo.h>

//Hardware Define
Adafruit_FeatherOLED oled = Adafruit_FeatherOLED();
Adafruit_MPU6050 mpu;
Servo myServo;

//Led Variables 
const int greenLed = 19;
const int yellowLed = 18;
const int redLed = 5;

// Servo Variables 
const int servoPin = 13;
int armedPos = 145;
int launchPos = 90;

#define WHITE    0xFFFF

void setup(){
  delay(10);

  Serial.begin(115200);
    ESP32PWM::allocateTimer(0);
	  ESP32PWM::allocateTimer(1);
	  ESP32PWM::allocateTimer(2);
	  ESP32PWM::allocateTimer(3);

  pinMode(greenLed, OUTPUT);
  pinMode(yellowLed, OUTPUT);
  pinMode(redLed, OUTPUT);

  myServo.attach(servoPin, 500, 2500);
  myServo.write(0);
 
  oled.init();

  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");
}
sensors_event_t event;

void loop(){

}