|
#include <Servo.h>
|
|
|
|
// Déclaration des servomoteurs
|
|
Servo servoBas; // Servomoteur pilotant l'azimuth
|
|
Servo servoHaut; // Servomoteur pilotant l'elevation
|
|
|
|
/////////////////////////////////
|
|
/// DECLARATION DES BROCHES ///
|
|
/////////////////////////////////
|
|
|
|
// PIN LDR (Sens: vue de face)
|
|
int LDR_HAUT = A3;
|
|
int LDR_BAS = A1;
|
|
int LDR_GAUCHE = A0;
|
|
int LDR_DROITE = A2;
|
|
|
|
//PIN LEDs
|
|
int LED_H = 7;
|
|
int LED_B = 4;
|
|
int LED_G = 6;
|
|
int LED_D = 5;
|
|
|
|
// PIN SERVOMOTEURS
|
|
int pin_servoBas = 9;
|
|
int pin_servoHaut = 10;
|
|
|
|
|
|
/////////////////////////////////
|
|
/// DECLARATION DES VARIABLES ///
|
|
/////////////////////////////////
|
|
|
|
// VARIABLES DE LECTURE DES LDRs
|
|
int LDR_H=0;
|
|
int LDR_B=0;
|
|
int LDR_G=0;
|
|
int LDR_D=0;
|
|
|
|
// Variables de calculs
|
|
int erreur = 30; // Erreur toleree entre les valeurs renvoyees par les photoresistances. Plus cette erreur est petite, plus le système sera nerveux (et risque d'être instable)
|
|
|
|
// Programme d'initialisation. S'execute une seule fois, en début de programme
|
|
void setup() {
|
|
|
|
// Les servomoteurs sont déclarés et une premiere valeur leur est donnee, afin d'eviter une initialisation trop brutale
|
|
servoBas.attach(pin_servoBas); // Attache le servo du bas
|
|
servoBas.write(90);
|
|
servoHaut.attach(pin_servoHaut); // Attache le servo du haut
|
|
servoHaut.write(160);
|
|
|
|
Serial.begin(9600); // Lance la communication serie a 9600 bits/s
|
|
|
|
/// DECLARATION DES ENTREES ET SORTIES ///
|
|
// Les LDR sont declarees comme des entrees:
|
|
pinMode(LDR_HAUT, INPUT);
|
|
pinMode(LDR_BAS, INPUT);
|
|
pinMode(LDR_GAUCHE, INPUT);
|
|
pinMode(LDR_DROITE, INPUT);
|
|
|
|
// Les LEDs sont declarees comme des sorties:
|
|
pinMode(LED_H, OUTPUT);
|
|
pinMode(LED_B, OUTPUT);
|
|
pinMode(LED_D, OUTPUT);
|
|
pinMode(LED_G, OUTPUT);
|
|
|
|
}
|
|
|
|
/////////////////////////////////
|
|
/// SOUS-FONCTIONS ///
|
|
/////////////////////////////////
|
|
|
|
// Fonction permettant de piloter un servo a partir d'une valeur donnee dans le programme
|
|
void w_servo(int servo, int val) {
|
|
switch(servo) { // Selection du servomoteur
|
|
case 0:
|
|
servoBas.write(limitS(0,val));
|
|
break;
|
|
case 1:
|
|
servoHaut.write(limitS(1,val));
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Fonction test des LDR (but: verifier le bon fonctionnement des LDR et fixer une erreur), allume les LEDs associees aux capteurs
|
|
void testLDR(){
|
|
|
|
//Lecture des valeurs renvoyees par les LDR:
|
|
LDR_H=analogRead(LDR_HAUT);
|
|
LDR_B=analogRead(LDR_BAS);
|
|
LDR_G=analogRead(LDR_GAUCHE);
|
|
LDR_D=analogRead(LDR_DROITE);
|
|
|
|
// Paire haut-bas
|
|
if (abs(LDR_H-LDR_B) > erreur) { // Si l'ecart entre les 2 LDR est significatif, on allume la LED correspondant a la LDR la plus eclairee
|
|
if (LDR_H > LDR_B) {
|
|
digitalWrite(LED_H, HIGH);
|
|
digitalWrite(LED_B, LOW);
|
|
}
|
|
else{
|
|
digitalWrite(LED_H, LOW);
|
|
digitalWrite(LED_B, HIGH);
|
|
}
|
|
}
|
|
else { // Sinon, on éteint les LEDs
|
|
digitalWrite(LED_H, LOW);
|
|
digitalWrite(LED_B, LOW);
|
|
}
|
|
|
|
// Paire gauche-droite
|
|
if (abs(LDR_G-LDR_D) > erreur) { // Si l'ecart entre les 2 LDR est significatif, on allume la LED correspondant a la LDR la plus eclairee
|
|
if (LDR_G > LDR_D) {
|
|
digitalWrite(LED_G, HIGH);
|
|
digitalWrite(LED_D, LOW);
|
|
}
|
|
else{
|
|
digitalWrite(LED_G, LOW);
|
|
digitalWrite(LED_D, HIGH);
|
|
}
|
|
}
|
|
else { // Sinon, on éteint les LEDs
|
|
digitalWrite(LED_G, LOW);
|
|
digitalWrite(LED_D, LOW);
|
|
}
|
|
|
|
|
|
}
|
|
|
|
// Asservissement en position des servomoteurs
|
|
void asserv() {
|
|
|
|
|
|
int angleActuel0;
|
|
int angleNouveau0=servoBas.read();
|
|
int angleActuel1;
|
|
int angleNouveau1=servoHaut.read();
|
|
|
|
//Lecture des valeurs renvoyees par les LDR:
|
|
LDR_H=analogRead(LDR_HAUT);
|
|
LDR_B=analogRead(LDR_BAS);
|
|
LDR_G=analogRead(LDR_GAUCHE);
|
|
LDR_D=analogRead(LDR_DROITE);
|
|
|
|
if(LDR_H < 400 && LDR_B < 400 && LDR_G < 400 && LDR_D < 400){ // cette boucle permet d'effectuer rotation uniquement s'il fait jour
|
|
w_servo(0,10);
|
|
w_servo(1,160); // s'il fait nuit, le robot se met en position pour attendre le soleil le lendemain matin
|
|
}
|
|
else{
|
|
// Paire haut-bas
|
|
if (abs(LDR_H-LDR_B) > erreur) {
|
|
// Si c'est plus lumineux en haut, il faut relever le panneau
|
|
if (LDR_H > LDR_B) {
|
|
angleActuel1 = servoHaut.read();
|
|
angleNouveau1 = angleActuel1-1; // Decrementer l'angle de commande = relever le panneau
|
|
}
|
|
else{
|
|
angleActuel1 = servoHaut.read();
|
|
angleNouveau1 = angleActuel1+1; // Incrementer l'angle = baisser le panneau
|
|
}
|
|
}
|
|
w_servo(1,angleNouveau1); // Pilotage du servomoteur.
|
|
|
|
// Paire gauche-droite
|
|
if (abs(LDR_G-LDR_D) > erreur) {
|
|
// Si c'est plus lumineux a gauche, il faut tourner le panneau vers la "droite"
|
|
if (LDR_G > LDR_D) {
|
|
angleActuel0 = servoBas.read();
|
|
angleNouveau0 = angleActuel0-1;
|
|
}
|
|
else{
|
|
angleActuel0 = servoBas.read();
|
|
angleNouveau0 = angleActuel0+1;
|
|
}
|
|
}
|
|
w_servo(0,angleNouveau0);
|
|
delay(50);
|
|
}
|
|
}
|
|
|
|
// Fonction de limite d'angle pour les servomoteurs
|
|
// Permet de fixer des butees angulaires et d'empêcher des collisions ou blocage/forçage mecanique
|
|
int limitS(int i, int angle) { // i=0 pour le servo du bas, i=1 pour le servo du haut
|
|
|
|
int angle2 = angle; // Nouvel angle de sortie
|
|
// Declaration de variables qui seront les limites angulaires du servomoteur (en degres)
|
|
int limBasse;
|
|
int limHaute;
|
|
|
|
// Definition des limites angulaires selon le servomoteur (en degrés)
|
|
switch(i) {
|
|
case 0: // Servomoteur du bas
|
|
limBasse = 5;
|
|
limHaute = 175;
|
|
break;
|
|
case 1: // Servomoteur du haut
|
|
limBasse = 90;
|
|
limHaute = 175;
|
|
break;
|
|
default: break;
|
|
}
|
|
|
|
// Si l'angle demande au servo depasse les limites, on fixe cet angle a la limite depassee
|
|
if (angle > limHaute) {
|
|
angle2 = limHaute;
|
|
}
|
|
if (angle < limBasse) {
|
|
angle2 = limBasse;
|
|
}
|
|
|
|
return angle2;
|
|
|
|
}
|
|
|
|
// FONCTIONS DE DEBUG
|
|
|
|
// Lecture et ecriture des valeurs renvoyees par les LED haut et bas
|
|
void lectureHB() {
|
|
LDR_H = analogRead(LDR_HAUT);
|
|
LDR_B = analogRead(LDR_BAS);
|
|
Serial.print(" , ");
|
|
Serial.print(LDR_H);
|
|
Serial.print(" , ");
|
|
Serial.print(LDR_B);
|
|
Serial.println();
|
|
delay(100);
|
|
}
|
|
|
|
// Lecture et ecriture des valeurs renvoyees par les LDR gauche et droite
|
|
void lectureGD() {
|
|
LDR_G = analogRead(LDR_GAUCHE);
|
|
LDR_D = analogRead(LDR_DROITE);
|
|
Serial.print(LDR_G);
|
|
Serial.print(", ");
|
|
Serial.print(LDR_D);
|
|
Serial.println();
|
|
|
|
}
|
|
|
|
void mesureangle(){
|
|
int angleActuel0=servoBas.read();
|
|
int angleActuel1=servoHaut.read();
|
|
|
|
Serial.print(angleActuel0);
|
|
Serial.print(" , ");
|
|
Serial.print(angleActuel1);
|
|
Serial.println();
|
|
delay(100);
|
|
|
|
}
|
|
/////////////////////////////////
|
|
/// FONCTION PRINCIPALE ///
|
|
/////////////////////////////////
|
|
|
|
void loop() {
|
|
asserv();
|
|
//lectureGD();
|
|
testLDR();
|
|
// lectureHB();
|
|
mesureangle();
|
|
}
|