/*

Auto-leveling platform
Instructions, parts list, and circuit diagram:
https://www-.sciencebuddies.org/science-fair-projects/project-ideas/Robotics_p067/robotics/auto-leveling-platform

*/

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

// create servo object
Servo servo1;

// accelerometer reading
int ax_ADC;

// reading when platform is level
const int setpoint = 340;

// servo angle variables
float angle = 90;
float change;
float error;

// proportional controller gain
const float Kp = 1;

void setup() { // put your setup code here, to run once:
  Serial.begin(9600);   // initialize serial
  servo1.attach(9);     // attach servo pin
  servo1.write(angle);  // set initial servo angle
}

void loop() { // put your main code here, to run repeatedly:
  // read accelerometer
  ax_ADC = analogRead(A0);

  // calculate error
  error = ax_ADC - setpoint;
  // calculate change
  change = Kp * error;

  angle = angle + change;

  angle = constrain(angle,0,180);

  servo1.write(angle);
  
  // Serial.print("ADC: ");
  // Serial.print(ax_ADC);
  // Serial.print(" | Error: ");
  // Serial.print(error);
  // Serial.print(" | Change: ");
  // Serial.print(change);
  // Serial.print(" | Angle: ");
  // Serial.println(angle);

}