#include <Servo.h>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Pin definitions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#define PIN_PIED_GAUCHE 11
#define PIN_PIED_DROIT 10
#define PIN_HANCHE_GAUCHE 9
#define PIN_HANCHE_DROITE 6
#define PIN_ECHO 2
#define PIN_TRIG 3
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Plage servo
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#define MAX_PIED_GAUCHE 178
#define MID_PIED_GAUCHE 99
#define MIN_PIED_GAUCHE 77
#define MAX_PIED_DROIT 120
#define MID_PIED_DROIT 82
#define MIN_PIED_DROIT 10
#define MAX_HANCHE_GAUCHE 120
#define MID_HANCHE_GAUCHE 85
#define MIN_HANCHE_GAUCHE 56
#define MAX_HANCHE_DROIT 113
#define MID_HANCHE_DROIT 87
#define MIN_HANCHE_DROIT 50
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Etats
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#define ETAT_EN_ATTENTE 0
#define ETAT_CHOIX_DANSE 1
#define ETAT_DANSE 2
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Danses
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#define MAX_DANSES 3
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// LISTE SERVOS
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#define SERVO_PIED_GAUCHE 0
#define SERVO_PIED_DROIT 1
#define SERVO_HANCHE_GAUCHE 2
#define SERVO_HANCHE_DROITE 3
#define NOMBRE_SERVO 4
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Variables
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Servo servoPiedGauche;
Servo servoPiedDroit;
Servo servoHancheGauche;
Servo servoHancheDroite;
byte randNumber = 0;
byte etat = ETAT_EN_ATTENTE;
Servo servos[NOMBRE_SERVO];
unsigned long animationStartTimestamps[NOMBRE_SERVO];
boolean animationFiniFlags[NOMBRE_SERVO];
byte pasDeDanseCourant = 0;
// danse[PAS_DE_DANSE][NOMBRE_SERVO] = {struct Course};
typedef struct {
byte angleDebut;
byte angleFin;
unsigned long duree;
} Course;
typedef Course Servos[NOMBRE_SERVO];
typedef Servos PasDanse0[6];
PasDanse0 danse0;
typedef Servos PasDanse1[6];
PasDanse1 danse1;
typedef Servos PasDanse2[4];
PasDanse2 danse2;
byte nombrePasParDanse[MAX_DANSES] = {6, 6, 4};
byte danseCourante;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Animations danses
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Permet de synchroniser les mouvements sur les 4 servos
*/
void resetAnimationFiniFlags() {
for(byte i=0; i<NOMBRE_SERVO; i++) {
animationFiniFlags[i] = false;
}
}
void resetAnimationStartTimestamps() {
for(byte i=0; i<NOMBRE_SERVO; i++) {
animationStartTimestamps[i] = 0;
}
}
/**
* retourne true si course finie
*/
boolean animeCourse(byte servoIndex, byte angleDebut, byte angleFin, unsigned long duree) {
// On fait rien si la course est déjà finie.
if(animationFiniFlags[servoIndex]) {
return true;
}
// Si la durée est 0, on fait juste un servo.write
if(duree == 0 || angleDebut == angleFin) {
servos[servoIndex].write(angleFin);
animationFiniFlags[servoIndex] = true;
return true;
}
// on conserve l'heure de démarrage de l'animation
if(animationStartTimestamps[servoIndex] == 0) {
animationStartTimestamps[servoIndex] = millis();
}
unsigned long timePosition = millis() - animationStartTimestamps[servoIndex];
// On map la position angulaire sur la durée de la course.
//long angle = map(animationStartTimestamp+timePosition, animationStartTimestamp, animationStartTimestamp+duree, angleDebut, angleFin);
long angle = map(timePosition, 0, duree, angleDebut, angleFin);
// On défini les bornes
if(angleFin > angleDebut) {
if(angle > angleFin) angle = angleFin;
}
else {
if(angle < angleFin) angle = angleFin;
}
servos[servoIndex].write(angle);
if(angle == angleFin) {
// reset animation
animationFiniFlags[servoIndex] = true;
return true;
}
return false;
}
/*
* Fonction d'adaptation des tableaux de pas de danse.
* Retourne la course du pas de danse courante, pour la danse courante et pour le servo donné.
*/
Course getCourseDeLaDanseCourante(byte servo) {
switch(danseCourante) {
case 0 :
return danse0[pasDeDanseCourant][servo];
case 1 :
return danse1[pasDeDanseCourant][servo];
case 2 :
return danse2[pasDeDanseCourant][servo];
}
return danse0[pasDeDanseCourant][servo];
}
/**
* retourne true si danse fini
*/
boolean execureDanse() {
Course course;
course = getCourseDeLaDanseCourante(SERVO_PIED_GAUCHE);
boolean finiPG = animeCourse(SERVO_PIED_GAUCHE, course.angleDebut, course.angleFin, course.duree);
course = getCourseDeLaDanseCourante(SERVO_PIED_DROIT);
boolean finiPD = animeCourse(SERVO_PIED_DROIT, course.angleDebut, course.angleFin, course.duree);
course = getCourseDeLaDanseCourante(SERVO_HANCHE_GAUCHE);
boolean finiHG = animeCourse(SERVO_HANCHE_GAUCHE, course.angleDebut, course.angleFin, course.duree);
course = getCourseDeLaDanseCourante(SERVO_HANCHE_DROITE);
boolean finiHD = animeCourse(SERVO_HANCHE_DROITE, course.angleDebut, course.angleFin, course.duree);
// Chaque pas est synchronisé.
if(finiHG && finiHD && finiPG && finiPD) {
resetAnimationFiniFlags();
resetAnimationStartTimestamps();
pasDeDanseCourant++;
if(pasDeDanseCourant >= nombrePasParDanse[danseCourante]) {
pasDeDanseCourant = 0;
return true;
}
}
return false;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Fonctions utilitaires
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
long lectureDistanceCm() {
digitalWrite(PIN_TRIG, HIGH);
delayMicroseconds(10);
digitalWrite(PIN_TRIG, LOW);
long lecture_echo = pulseIn(PIN_ECHO, HIGH, 60000); // 60ms
return(lecture_echo / 58);
}
void position0() {
servoPiedGauche.write(MID_PIED_GAUCHE);
servoPiedDroit.write(MID_PIED_DROIT);
servoHancheGauche.write(MID_HANCHE_GAUCHE);
servoHancheDroite.write(MID_HANCHE_DROIT);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// MACHINE ETATS
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Etat : en attente
* retourne l'état en attente ou choix danse
*/
byte decideSiDanse() {
long distanceCm = lectureDistanceCm();
if(distanceCm <= 10 || distanceCm >= 100) {
position0();
delay(500);
return ETAT_EN_ATTENTE;
}
return ETAT_CHOIX_DANSE;
}
/**
* Etat : choix danse
* retourne l'état danse
*/
byte choisirDanse() {
danseCourante = random(MAX_DANSES);
return ETAT_DANSE;
}
/**
* Etat : danse
* retourne etat En attente ou danse
*/
byte danse() {
boolean termine = execureDanse();
if(termine) {
delay(50);
return ETAT_EN_ATTENTE;
}
return ETAT_DANSE;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// SETUP
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void setup() {
// Setup servos
servoPiedGauche.attach(PIN_PIED_GAUCHE);
servoPiedDroit.attach(PIN_PIED_DROIT);
servoHancheGauche.attach(PIN_HANCHE_GAUCHE);
servoHancheDroite.attach(PIN_HANCHE_DROITE);
// Tiens-toi droit Bob !
position0();
// Setup capteur US
pinMode(PIN_TRIG, OUTPUT);
digitalWrite(PIN_TRIG, LOW);
pinMode(PIN_ECHO, INPUT);
// Seed random
randomSeed(analogRead(0));
// Init du tableau de servos
servos[SERVO_PIED_GAUCHE] = servoPiedGauche;
servos[SERVO_PIED_DROIT] = servoPiedDroit;
servos[SERVO_HANCHE_GAUCHE] = servoHancheGauche;
servos[SERVO_HANCHE_DROITE] = servoHancheDroite;
resetAnimationFiniFlags();
resetAnimationStartTimestamps();
// Init des danses
/*
danse[][SERVO_PIED_GAUCHE] = {};
danse[][SERVO_PIED_DROIT] = {};
danse[][SERVO_HANCHE_GAUCHE] = {};
danse[][SERVO_HANCHE_DROITE] = {};
*/
//danse / pas / servo / course
danse0[0][SERVO_PIED_GAUCHE] = {MID_PIED_GAUCHE, MAX_PIED_GAUCHE, 1000};
danse0[0][SERVO_PIED_DROIT] = {MID_PIED_DROIT, MAX_PIED_DROIT, 1000};
danse0[0][SERVO_HANCHE_GAUCHE] = {MID_HANCHE_GAUCHE, MID_HANCHE_GAUCHE, 0};
danse0[0][SERVO_HANCHE_DROITE] = {MID_HANCHE_DROIT, MID_HANCHE_DROIT, 0};
danse0[1][SERVO_PIED_GAUCHE] = {MAX_PIED_GAUCHE, MIN_PIED_GAUCHE, 500};
danse0[1][SERVO_PIED_DROIT] = {MAX_PIED_DROIT, MAX_PIED_DROIT, 0};
danse0[1][SERVO_HANCHE_GAUCHE] = {MID_HANCHE_GAUCHE, MID_HANCHE_GAUCHE, 0};
danse0[1][SERVO_HANCHE_DROITE] = {MID_HANCHE_DROIT, MID_HANCHE_DROIT, 0};
danse0[2][SERVO_PIED_GAUCHE] = {MIN_PIED_GAUCHE, MID_PIED_GAUCHE, 100};
danse0[2][SERVO_PIED_DROIT] = {MAX_PIED_DROIT, MAX_PIED_DROIT, 0};
danse0[2][SERVO_HANCHE_GAUCHE] = {MID_HANCHE_GAUCHE, MID_HANCHE_GAUCHE, 0};
danse0[2][SERVO_HANCHE_DROITE] = {MID_HANCHE_DROIT, MID_HANCHE_DROIT, 0};
danse0[3][SERVO_PIED_GAUCHE] = {MID_PIED_GAUCHE, MIN_PIED_GAUCHE, 100};
danse0[3][SERVO_PIED_DROIT] = {MAX_PIED_DROIT, MAX_PIED_DROIT, 0};
danse0[3][SERVO_HANCHE_GAUCHE] = {MID_HANCHE_GAUCHE, MID_HANCHE_GAUCHE, 0};
danse0[3][SERVO_HANCHE_DROITE] = {MID_HANCHE_DROIT, MID_HANCHE_DROIT, 0};
danse0[4][SERVO_PIED_GAUCHE] = {MIN_PIED_GAUCHE, MID_PIED_GAUCHE, 100};
danse0[4][SERVO_PIED_DROIT] = {MAX_PIED_DROIT, MAX_PIED_DROIT, 0};
danse0[4][SERVO_HANCHE_GAUCHE] = {MID_HANCHE_GAUCHE, MID_HANCHE_GAUCHE, 0};
danse0[4][SERVO_HANCHE_DROITE] = {MID_HANCHE_DROIT, MID_HANCHE_DROIT, 0};
danse0[5][SERVO_PIED_GAUCHE] = {MID_PIED_GAUCHE, MID_PIED_GAUCHE, 0};
danse0[5][SERVO_PIED_DROIT] = {MAX_PIED_DROIT, MID_PIED_DROIT, 1500};
danse0[5][SERVO_HANCHE_GAUCHE] = {MID_HANCHE_GAUCHE, MID_HANCHE_GAUCHE, 0};
danse0[5][SERVO_HANCHE_DROITE] = {MID_HANCHE_DROIT, MID_HANCHE_DROIT, 0};
danse1[0][SERVO_PIED_GAUCHE] = {MID_PIED_GAUCHE, MIN_PIED_GAUCHE, 1000};
danse1[0][SERVO_PIED_DROIT] = {MID_PIED_DROIT, MIN_PIED_DROIT, 1000};
danse1[0][SERVO_HANCHE_GAUCHE] = {MID_HANCHE_GAUCHE, MID_HANCHE_GAUCHE, 0};
danse1[0][SERVO_HANCHE_DROITE] = {MID_HANCHE_DROIT, MID_HANCHE_DROIT, 0};
danse1[1][SERVO_PIED_GAUCHE] = {MIN_PIED_GAUCHE, MIN_PIED_GAUCHE, 0};
danse1[1][SERVO_PIED_DROIT] = {MIN_PIED_DROIT, MID_PIED_DROIT, 1000};
danse1[1][SERVO_HANCHE_GAUCHE] = {MID_HANCHE_GAUCHE, MID_HANCHE_GAUCHE, 0};
danse1[1][SERVO_HANCHE_DROITE] = {MID_HANCHE_DROIT, MID_HANCHE_DROIT, 0};
danse1[2][SERVO_PIED_GAUCHE] = {MIN_PIED_GAUCHE, MIN_PIED_GAUCHE, 0};
danse1[2][SERVO_PIED_DROIT] = {MID_PIED_DROIT, MID_PIED_DROIT, 0};
danse1[2][SERVO_HANCHE_GAUCHE] = {MID_HANCHE_GAUCHE, MAX_HANCHE_GAUCHE, 100};
danse1[2][SERVO_HANCHE_DROITE] = {MID_HANCHE_DROIT, MID_HANCHE_DROIT, 0};
danse1[3][SERVO_PIED_GAUCHE] = {MIN_PIED_GAUCHE, MIN_PIED_GAUCHE, 0};
danse1[3][SERVO_PIED_DROIT] = {MID_PIED_DROIT, MID_PIED_DROIT, 0};
danse1[3][SERVO_HANCHE_GAUCHE] = {MAX_HANCHE_GAUCHE, MIN_HANCHE_GAUCHE, 100};
danse1[3][SERVO_HANCHE_DROITE] = {MID_HANCHE_DROIT, MID_HANCHE_DROIT, 0};
danse1[4][SERVO_PIED_GAUCHE] = {MIN_PIED_GAUCHE, MIN_PIED_GAUCHE, 0};
danse1[4][SERVO_PIED_DROIT] = {MID_PIED_DROIT, MID_PIED_DROIT, 0};
danse1[4][SERVO_HANCHE_GAUCHE] = {MIN_HANCHE_GAUCHE, MAX_HANCHE_GAUCHE, 100};
danse1[4][SERVO_HANCHE_DROITE] = {MID_HANCHE_DROIT, MID_HANCHE_DROIT, 0};
danse1[5][SERVO_PIED_GAUCHE] = {MIN_PIED_GAUCHE, MID_PIED_GAUCHE, 1000};
danse1[5][SERVO_PIED_DROIT] = {MID_PIED_DROIT, MID_PIED_DROIT, 0};
danse1[5][SERVO_HANCHE_GAUCHE] = {MAX_HANCHE_GAUCHE, MID_HANCHE_GAUCHE, 1000};
danse1[5][SERVO_HANCHE_DROITE] = {MID_HANCHE_DROIT, MID_HANCHE_DROIT, 0};
danse2[0][SERVO_PIED_GAUCHE] = {MID_PIED_GAUCHE, MID_PIED_GAUCHE, 0};
danse2[0][SERVO_PIED_DROIT] = {MID_PIED_DROIT, MID_PIED_DROIT, 0};
danse2[0][SERVO_HANCHE_GAUCHE] = {MID_HANCHE_GAUCHE, MAX_HANCHE_GAUCHE, 1000};
danse2[0][SERVO_HANCHE_DROITE] = {MID_HANCHE_DROIT, MAX_HANCHE_DROIT, 1000};
danse2[1][SERVO_PIED_GAUCHE] = {MID_PIED_GAUCHE, MID_PIED_GAUCHE, 0};
danse2[1][SERVO_PIED_DROIT] = {MID_PIED_DROIT, MID_PIED_DROIT, 0};
danse2[1][SERVO_HANCHE_GAUCHE] = {MAX_HANCHE_GAUCHE, MIN_HANCHE_GAUCHE, 500};
danse2[1][SERVO_HANCHE_DROITE] = {MAX_HANCHE_DROIT, MIN_HANCHE_DROIT, 500};
danse2[2][SERVO_PIED_GAUCHE] = {MID_PIED_GAUCHE, MID_PIED_GAUCHE, 0};
danse2[2][SERVO_PIED_DROIT] = {MID_PIED_DROIT, MID_PIED_DROIT, 0};
danse2[2][SERVO_HANCHE_GAUCHE] = {MIN_HANCHE_GAUCHE, MAX_HANCHE_GAUCHE, 500};
danse2[2][SERVO_HANCHE_DROITE] = {MIN_HANCHE_DROIT, MAX_HANCHE_DROIT, 500};
danse2[3][SERVO_PIED_GAUCHE] = {MID_PIED_GAUCHE, MID_PIED_GAUCHE, 0};
danse2[3][SERVO_PIED_DROIT] = {MID_PIED_DROIT, MID_PIED_DROIT, 0};
danse2[3][SERVO_HANCHE_GAUCHE] = {MAX_HANCHE_GAUCHE, MID_HANCHE_GAUCHE, 1000};
danse2[3][SERVO_HANCHE_DROITE] = {MAX_HANCHE_DROIT, MID_HANCHE_DROIT, 1000};
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// LOOP
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void loop() {
switch(etat) {
case ETAT_EN_ATTENTE :
etat = decideSiDanse();
break;
case ETAT_CHOIX_DANSE :
etat = choisirDanse();
break;
case ETAT_DANSE :
etat = danse();
break;
}
}