//AS5600 rotary encoder magnetico + 2 servo da 180°

//Servo1 180°-0° (passando da 270°) D6
//Servo2 0°-180° (passando da 90°) D7

//AS5600 SDA A4 - SCL A5

//#include "TFT.h"
#include "SPI.h"
#include "Servo.h"

#include "AS5600.h"
#include "Wire.h"

#define TFT_DC 9
#define TFT_CS 10
#define TFT_RST 8
#define TFT_MOSI 11
#define TFT_CLK 13
#define TFT_MISO 12

#define PIN_SERVO1 6
#define PIN_SERVO2 7


AS5600 as5600;
int ang, lang = 0;

int direction;
int direction_180; //direzione per vela da 0 a +- 180
float directionRad;

//PER SENSORE
Servo Servo1; //Il nome del servo è Servo1
Servo Servo2; //Il nome del servo è Servo1


void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
Serial.println("Direzione vento\tMPH");
Servo1.attach(PIN_SERVO1);
Servo2.attach(PIN_SERVO2);
Serial.begin(115200);
Wire.begin();
Servo1.write(0);
Servo2.write(0);


Serial.print("detectMagnet: ");
Serial.println(as5600.detectMagnet());

 /*if(as5600.detectMagnet() == 0 ){
    while(1){
        if(as5600.detectMagnet() == 1 ){
            Serial.print("Current Magnitude: ");
            Serial.println(as5600.readMagnitude());
            break;
        }
        else{
            Serial.println("Can not detect magnet");
        }
        delay(1000);
    }
    
  }
*/
}

int CalcolaDirezione()
{
  // DIREZIONE
Serial.println("as5600.getRawAngle():");
Serial.println(String(convertRawAngleToDegrees(as5600.readAngle()),DEC));

//int sensorValue = analogRead(A0);
//  float voltage = sensorValue*5/1023.0;
//  direction = map(sensorValue, 0, 1023, 0, 360);
//direction =convertRawAngleToDegrees(as5600.readAngle());
direction = direction + 10;
if (direction > 360) direction=0;

  if (direction>180) 
    direction_180 = abs(direction-360);
  else
    direction_180 = direction;


Serial.print("Calcola direzione, direction: ");
Serial.println(direction);
Serial.print("Calcola direzione, direction_180: ");
Serial.println(direction_180);

return (int)direction;
}

void loop() {

//carcolo in radianti e disegno la linea
directionRad = direction * PI/180;


PosizionaLancetta(CalcolaDirezione());
delay(500);
}



float convertRawAngleToDegrees(word newAngle)
{
  /* Raw data reports 0 - 4095 segments, which is 0.087890625 of a degree */
  float retVal = newAngle * 0.087890625;
  return retVal;
}

   

void PosizionaLancetta(int gradi)
{

  //gradi=0;
Serial.println("Servo gradi:");
Serial.println(gradi);


if (gradi>=0 && gradi <= 180){
  Servo2.write (180);
  Servo1.write (abs(gradi-180));
}
if (gradi>180 && gradi <= 360){
  Servo1.write (0);
  Servo2.write (abs(gradi - 360));
}

//Servo1.write (0);
//Servo2.write (0);
}