#include <Encoder.h>
#include <Servo.h>


Encoder enc[2] = {Encoder(3,4),Encoder(6,7)};

Servo motor[2];

double angle[2];

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  motor[0].attach(2);
  motor[1].attach(5);
}

void loop() {
  // put your main code here, to run repeatedly:
  control(0);
  delay(10);
  control(1);

  Serial.print(angle[0]); Serial.print(' '); Serial.println(angle[1]);
  delay(10);
}

void control(int n){
  int pulse = enc[n].read();
  angle[n] = (double)pulse/80*2*PI;
  int angle_deg = constrain(angle[n]*180/PI+90,0,180);

  motor[n].write(angle_deg); 
}

void Ball(){

}