#include "definitions.h"
#include "vacuum.h"
#include "servo.h"

void setup() {
  Serial.begin(115200);

  pinMode(PIN_BUTTON,INPUT_PULLDOWN);

  initVacuum();


  pinMode(PIN_INPUTARM1,INPUT);

  initServos();
}







void loop() {
  unsigned long loopmillis=millis();

  bool vac=digitalRead(PIN_BUTTON); //Temporary Vacuum Button
  setVacuum(vac);

  loopVacuum(loopmillis);

  loopServos(loopmillis);


  //Print Debug Information
  static unsigned long last_print=0;
  if (loopmillis - last_print >100) {
    last_print=loopmillis;
    //printVacuumValues();
    //Serial.print(" Vac="); Serial.print(vac);

    printServoDebug();
    
    
    Serial.println();
    Serial.println();
  }

}