#include "Servo.h"

/******************************************************************************
 * servo-test.ino
 * @title: Test de pilotage d'un servomoteur 
 * @project: Blender-EduTech - Tutoriel 6 : Labyrinthe à bille - Développer le jumeau numérique du labyrinthe 
 * @lang: fr
 * @authors: Philippe Roy <philippe.roy@ac-grenoble.fr>
 * @copyright: Copyright (C) 2023 Philippe Roy 
 * @license: GNU GPL
 ******************************************************************************/

/****************************************************************************** 
* Servomoteur
******************************************************************************/

Servo servo;
String servo_txt;
String servo_txt2;
bool servo_sens;
int angle_ms;

/******************************************************************************
 * Initialisation
 ******************************************************************************/
   
void setup() {

    // Liaison série
    Serial.begin(115200);

    // Servomoteur
    servo.attach(6); // Servomoteur sur broche D9
    servo_sens = true;
    angle_ms = 600; // 500 -> 0°, 1500 -> 90° et 2500-> 180°
    servo.writeMicroseconds(angle_ms); // 600 -> 9°
    /* servo.write(10); */
}

/******************************************************************************
 * Boucle principale
 ******************************************************************************/

void loop() {

    // Affichage
    servo_txt = String(servo.read());
    servo_txt2 = String(angle_ms);
    Serial.print(servo_txt2);
    Serial.print(" -> ");
    Serial.print(servo_txt);
    Serial.println("°");

    // Sens
    if (servo.read()==90) {
	    servo_sens=false;
	}
    if (servo.read()==10) {
	    servo_sens=true;
	}

    // Mouvement
    if (servo_sens) {
	angle_ms=angle_ms+1; // Résolution à 180/2000 soit 0,09°
	/* servo.write(servo.read()+1); // Résolution à 1° */
	/* delay(100); */
    } else {
	angle_ms=angle_ms-1; // Résolution à 180/2000 soit 0,09°
	/* servo.write(servo.read()-1); // Résolution à 1° */
	/* delay(100); */
    }
    servo.writeMicroseconds(angle_ms);
    delay(5);

    // Test des positions
    /* angle_ms =1500; */
    /* servo.writeMicroseconds(angle_ms); */
    /* servo_txt = String(servo.read()); */
    /* servo_txt2 = String(angle_ms); */
    /* Serial.print(servo_txt2); */
    /* Serial.print(" -> "); */
    /* Serial.print(servo_txt); */
    /* Serial.println("°"); */
    /* delay(1000); */

    /* angle_ms =500; */
    /* servo.writeMicroseconds(angle_ms); */
    /* servo_txt = String(servo.read()); */
    /* servo_txt2 = String(angle_ms); */
    /* Serial.print(servo_txt2); */
    /* Serial.print(" -> "); */
    /* Serial.print(servo_txt); */
    /* Serial.println("°"); */
    /* delay(1000); */

    /* angle_ms =2500; */
    /* servo.writeMicroseconds(angle_ms); */
    /* servo_txt = String(servo.read()); */
    /* servo_txt2 = String(angle_ms); */
    /* Serial.print(servo_txt2); */
    /* Serial.print(" -> "); */
    /* Serial.print(servo_txt); */
    /* Serial.println("°"); */
    /* delay(1000); */
    
}