/*
 * questo programma deriva da
 * file:///home/giorgio/Dropbox/ProgettiStrutturati/Esoscheletro/ReactionWheel/Software/SingleReactionWheelImu/SingleReactionWheelImu.ino
 * da cui ha preso le inizializzazioni dei pin e il setup, ed è il motivo per cui in questo header ci sono cose potenzialmente
 * non utlizzate nel resto del programma ma è solo un software di parametrizzazione quindi non è di produzione
 * va bene così
 * Un altra fonte da cui sono state presi dei pezzi di codice relativi alla comunicazione seriale è:
 * file:///home/giorgio/Dropbox/ProgettiStrutturati/Esoscheletro/invio_ricezione_seriale/arduino-py-01/arduino-py-01.ino
 *
 *
 * L'obiettivo di questo software è capire in dettaglio il comportamento del motore con attaccata su la ruota di reazione
 * il gruppo motore/ruota è quello identificato da RW2 (ReactioWheel #2)
 *
 * Uno script python trasferità via seriale il valore pwm, arduino eseguirà il comando e ritornerà un valore medio
 * della velocità del motore.
 * L'idea è quella di avere un dataset completo di velocità-pwm
 * Successivamente il sistema di potrà estendere a accelerazioni-tempo in modo da avre i dati alla prima derivata
 * Calcolare il momento angolare e la forza risultante con tanta tanta fortuna
 *
 */

//definisco gli input per pilotare il motore
//2a e 2b danno la direzione motorPWM da il valore del pwm di uscita
#define motor2A   8
#define motor2B   7
#define motorPWM  9
//questo è il pin dove è attaccato il sensore orrico di RW2
#define RW2 2

//variabili per il calcolo della velocità usate nella funziona di interrupt
volatile int holesRW2 = 0;
int InterruptForCompleteTurn = 24;

//*******************  FUNZIONE CALLBACK PER L'INTERRUPT ****************************
//***********************************************************************************
//ad ogni cambio di stato del pin incrementa di 1 la variabile che conta i fori
void PassageRW2() {
  holesRW2 = holesRW2 + 1;
}


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.flush();

  //faccio un giochino di luci per identificare il fatto che il sistema è pronto e per
  //iniziliazzare l'output 13
  pinMode(13, OUTPUT);
  for(int i=0; i<100; i++) {
  digitalWrite(13, HIGH);
  delay(20);
  digitalWrite(13, LOW);
  delay(20);
  }
  digitalWrite(13, HIGH);
  delay(1000);

  //setto i pin relativi per controllare la direzione del motore
  pinMode(motor2A, OUTPUT);
  pinMode(motor2B, OUTPUT);
  //imposto che siano uno l'opposto dell'altro per farlo girare in una direzione qualsiasi
  digitalWrite(motor2A, HIGH);
  digitalWrite(motor2B, LOW);
  //setto il pin per controllare l'enable del motore
  pinMode(motorPWM, OUTPUT);
  //una volta settato porto il pwm a 0 per fermare la rotazione, se sta girando durante un reset,
  //do tempo di fermarsi o cmq di rallentare
  analogWrite(motorPWM, 0);
  //ed aspetto 3 secondi per farlo fermare
  delay(3000);
  analogWrite(motorPWM, 30);
  digitalWrite(13, LOW);

//aggancio la funzione di loopback dell'interrupt per il conteggio dei fori
attachInterrupt (digitalPinToInterrupt(RW2), PassageRW2, CHANGE);
}

void loop() {
  // put your main code here, to run repeatedly:
//leggo i dati dalla seriale, per essere sicuro del funzionamento della lettura disabilito e riabilito gli interrupt
int DatiRicevuti = ReadDataFromSerial();

//faccio partire il cronometro
unsigned long int now = micros();
//aspetto che la ruota faccia due giri ma mentre aspetto cmq leggo gli input per poter modificare
//al volo la velocità
while (holesRW2 < InterruptForCompleteTurn) { }
unsigned long int RevolutionTime = micros()-now;



//se la funzione rileva che ci sono dati nella seriale diversi da zero
///imposta il valore del pwm a quel livello
noInterrupts();
if (DatiRicevuti > 0){
  Serial.print("' valore contato: '");Serial.print(int(holesRW2));Serial.print(" tempo ='");Serial.print(String(RevolutionTime));Serial.print("' Imposto valore PWM a : '");Serial.println(String(DatiRicevuti));
  analogWrite(motorPWM, DatiRicevuti);
  }

//se i dati sono ricevuti sono minori o uguali a zero resta in attesa e porta a 0 il pwm
eser = serial.Serial('/dev/ttyUSB0', baudrate=115200)

print("azzero il pwm e aspetto 5 sec")
ser.write(str.encode("0"))
time.sleep(3)
print("avvio")

while True:
  for numero in range(50):
    # leggi cosa torna indietro e stampalo

    # questa funzione invia il dato
    ser.write(str.encode(str(numero + 20)))
    #delay di 4 secondi
    time.sleep(4)
    #pulisco i buffer
    ser.flushInput()
    ser.flushOutput()
    #menorizzo in una stringa leggo quello che arduino mi fa tornare indietro
    ser_bytes = ser.readline()
    #stampo la stringa
    print("dati letto: %s"%ser_bytes)
    #pulisco i buffer
    ser.flushInput()
    ser.flushOutput()