balance_bot.cpp

#include <FiniteStateMachine.h>
#include <Servo.h>

State LeaningLeft = State(correctLeftLean);
State LeaningRight = State(correctRightLean);
FSM balancer = FSM(LeaningRight);

Servo servoMotor;
int servoPin = 6;
int accelPin = 1;


void setup(){
  servoMotor.attach(servoPin);
  pinMode(accelPin, INPUT);
  Serial.begin(9600);
  
  servoMotor.write(90);
}

void loop(){
  int val = analogRead(accelPin);
  Serial.println(val);
  
  if(val > 350){
      balancer.transitionTo(LeaningRight);

  } else {
      balancer.transitionTo(LeaningLeft);
  }
  
  balancer.update();
}

void correctLeftLean(){
  Serial.println("correctLeft");
  servoMotor.write(servoMotor.read() - 3);
}

void correctRightLean(){
   Serial.println("correctRight");
   servoMotor.write(servoMotor.read() + 3);
}