#include <SPI.h>
#include <Wire.h>
#include <AS5048A.h>
#include <Servo.h>

const int CSN = D7;
const int SERVO_PIN = D0;

AS5048A angleSensor(CSN, true);
Servo bilda_servo;

void setup()
{
  pinMode(CSN, INPUT);
  pinMode(SERVO_PIN, OUTPUT);
  bilda_servo.attach(SERVO_PIN, 500, 2500);

	Serial.begin(19200);
	angleSensor.begin();
}

void loop()
{
	delay(100);

	float val = angleSensor.getRotationInDegrees();
  Serial.print("Rotation:");
  Serial.print(val);
  Serial.print(",");
  Serial.print("Lower:");
  Serial.print(0);
  Serial.print(",");
  Serial.print("Upper:");
  Serial.println(360);
  float pwm_width = map(val, 0, 360, 500, 2500);
  bilda_servo.writeMicroseconds(pwm_width);
  delay(20);
	// Serial.print("\nGot rotation of: ");
	// Serial.println(val);
	// Serial.println("State: ");
	// angleSensor.printState();
	// Serial.println("Errors: ");
	// Serial.println(angleSensor.getErrors());
	// Serial.println("Diagnostics: ");
	// Serial.println(angleSensor.getDiagnostic());
}