// Castoo
// Module de tests
// Affichage délai en microsecondes des parametres d'un servo moteur en test
// Affichage distance d'un capteur SR04 en test
// Sur ecran LCD

#include "LiquidCrystal.h"

const boolean test_serie = LOW; // Pour test avec sortie sur interface serie HIGH (A passer à LOW avant compilation definitive)
const int bt_typ_mesur = 11; // Bouton choix type de mesure
const int servo_a_tester = 2; // Broche S du servo a tester sur broche 2
const int potar_servo = 0; // Potentiometre de 10K pour faire varier le delai entre deux tests d'impulsion
const int trigPin = 8; // Broche du triger du capteur sr04
const int echoPin = 7; // Broche echo du capteur sr04
const float v = 331.5+0.6*20; // Calcul de la vitesse en seconde du son en fonction d'une temperature de 20 degres
LiquidCrystal lcd(9,10,3,4,5,6); // LCD avec liaison 4 bits de données
int v_potar; // Valeur analogique de lecture du potar du servo
char message[16] = ""; // Variable affichage sur ecran LCD 16 caracteres

void setup(){
	if (test_serie == HIGH) Serial.begin(115200); // Test
	lcd.begin(16,2); // Initialisation de l'écran
	pinMode(bt_typ_mesur, INPUT); // Bouton choix type de mesure
	digitalWrite(bt_typ_mesur, HIGH);	// mode pull-up sur bouton
	pinMode(trigPin, OUTPUT); // Broche triger en sortie
	pinMode(echoPin, INPUT); // Broche echo en entree
	pinMode(servo_a_tester, OUTPUT);
}

// Mesure de la distance et convertion en cm
float distanceM(){
	// Genere une impulsion de 10 microsecondes sur la broche triger
	digitalWrite(trigPin, LOW);
	delayMicroseconds(3);
	digitalWrite(trigPin, HIGH);
	delayMicroseconds(10);
	digitalWrite(trigPin, LOW);
	float tUs = pulseIn(echoPin, HIGH); 	// Lecture du temps alle/retour de l'onde en microsecondes
	float t = tUs / 1000.0 / 1000.0 / 2; 	// Conversion en secondes et div par deux (seulement le retour de l'onde)
	float d = t*v; 							// calcul de la vitesse en m/s
	return d*100; 							// convertion en cm pour affichage
}

// Test le servo en generant un etat haut pendant le temps passé en parametre
void test_Servo(int servo_a_tester, int test_delai_servo)	// <1>
{
	digitalWrite(servo_a_tester, HIGH);	// <2>
	delayMicroseconds(test_delai_servo);	// <3>
	digitalWrite(servo_a_tester, LOW);	// <4>
}

void loop(){
	// Affichage distance sur LCD
	lcd.clear();
	lcd.home(); // Replace le curseur en haut a gauche
	if(digitalRead(bt_typ_mesur) == HIGH){
		// Mesure de distance
		lcd.print("Mesure distance");
		// Appel fonction de lecture de la distance
		int d=distanceM();
		// Affichage distance sur console serie
		if (test_serie == HIGH) Serial.println(d, DEC); // Test
		if(d > 286 or d < 1){
			//si la distance est > a 2m86, l'onde est perdue
			lcd.setCursor(0,1);
			lcd.print(" Echec  mesure");
		}else{
			sprintf(message, "Dist : %5d cm", d);
			lcd.print(message);
		}
		delay(200); // ms
	}else{
		// mesure de servo moteur
		for (int i=300; i<=3000; i = i + 2) {
			test_Servo(servo_a_tester, i);
			v_potar = analogRead(potar_servo); // Lecture du potar du servo
			v_potar = map(v_potar, 0, 1023, 0, 150); // Convertion lecture du potar servo vers valeur entre 0 et 150
			if (test_serie == HIGH) Serial.println(v_potar); // Test
			delay(v_potar); // permet la lecture des valeurs sinon ca va trop vite
			if (test_serie == HIGH) Serial.println(i); // Test
			sprintf(message, "MicroSec: %5d", i);
			lcd.clear();
			lcd.home();
			lcd.print("Mesure Servo");
			lcd.setCursor(0,1);
			lcd.print(message);
			if(digitalRead(bt_typ_mesur) == HIGH) i=3001; // Si bouton choix mesure changé on sort boucle for
		}
	}
}