Commit 3ab1f346e425faae1130d6453a06ffd71050243f

Authored by rfoucaul
1 parent e9250d90

Modification finale de la partie programmation pour le rendu

Programmation/Deplacement.ino 0 → 100644
@@ -0,0 +1,344 @@ @@ -0,0 +1,344 @@
  1 +/*
  2 +==============================================================================================================
  3 +Pour que le programme fonctionne correctement les servos moteurs doivent être connecté de la façon suivante :
  4 +Pour une patte les 3 servos doivent se suivrent (ex : pin0 ,pin1, pin2)
  5 +Les tibia doivent être sur les pins 0 - 3 - 6 - 9 - 12 - 15
  6 +Les femur doivent être sur les pins 1 - 4 - 7 - 10 - 13
  7 +Les coxa doivent être sur les pins 2 - 5 - 8 - 11 - 14
  8 +==============================================================================================================
  9 + */
  10 +
  11 +#include <Wire.h>
  12 +#include <Servo.h>
  13 +#include <Adafruit_PWMServoDriver.h>
  14 +
  15 +Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
  16 +Servo servo16; //femur patte 5
  17 +Servo servo17; //coxa patte 5
  18 +
  19 +#define TIBIA_MIN 150 //tibia en haut
  20 +#define TIBIA_MAX 400 //tibia en bas
  21 +#define TIBIA_CALIBRATE 220 //62°
  22 +
  23 +#define FEMUR_MIN 200 //200
  24 +#define FEMUR_MAX 490 //500
  25 +#define FEMUR_CALIBRATE 420 //115°
  26 +
  27 +#define COXA_MIN 100 //coxa à gauche
  28 +#define COXA_MAX 550 //coxa à droite
  29 +#define COXA_CALIBRATE 320 //90°
  30 +
  31 +#define AVANT 0
  32 +#define AVANT_GAUCHE 1
  33 +#define AVANT_DROITE 5
  34 +#define ARRIERE 3
  35 +#define ARRIERE_GAUCHE 4
  36 +#define ARRIERE_DROITE 2
  37 +
  38 +struct legPart { //Structure donnant accées aux paramètres d'une patte
  39 + int legMin;
  40 + int legMax;
  41 + int legCalibrate;
  42 +} legPart;
  43 +
  44 +struct legPart tibia;
  45 +struct legPart femur;
  46 +struct legPart coxa;
  47 +
  48 +int patte[6][3]; //chaque patte est un tableau contenant le numero du pin pour tibia/femur/coxa. Ex : acceder au pin du gémur patte 2 -> patte[1][0]
  49 +
  50 +struct capteurs { //structure donnant accées aux valeurs des 3 capteurs
  51 + int avant;
  52 + int droite;
  53 + int gauche;
  54 +} capteurs;
  55 +
  56 +struct capteurs cap;
  57 +
  58 +//capteur ultrason
  59 +const byte TRIGGER_AVANT = 2; // Broche TRIGGER
  60 +const byte ECHO_AVANT = 3; // Broche ECHO
  61 +const byte TRIGGER_DROITE = 4; // Broche TRIGGER
  62 +const byte ECHO_DROITE = 5; // Broche ECHO
  63 +const byte TRIGGER_GAUCHE = 6; // Broche TRIGGER
  64 +const byte ECHO_GAUCHE = 7; // Broche ECHO
  65 +const unsigned long MEASURE_TIMEOUT = 25000UL; // 25ms = ~8m à 340m/s
  66 +const float SOUND_SPEED = 340.0 / 1000;
  67 +
  68 +void setup()
  69 +{
  70 + pinMode(TRIGGER_AVANT, OUTPUT);
  71 + pinMode(TRIGGER_DROITE, OUTPUT);
  72 + pinMode(TRIGGER_GAUCHE, OUTPUT);
  73 + digitalWrite(TRIGGER_AVANT, LOW); // La broche TRIGGER doit être à LOW au repos
  74 + digitalWrite(TRIGGER_DROITE, LOW); // La broche TRIGGER doit être à LOW au repos
  75 + digitalWrite(TRIGGER_GAUCHE, LOW); // La broche TRIGGER doit être à LOW au repos
  76 + pinMode(ECHO_AVANT, INPUT);
  77 + pinMode(ECHO_DROITE, INPUT);
  78 + pinMode(ECHO_GAUCHE, INPUT);
  79 +
  80 +
  81 + servo16.attach(10); //femur
  82 + servo17.attach(11); //coxa
  83 + Serial.begin(9600);
  84 + pwm.begin();
  85 + pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates
  86 +
  87 + tibia={TIBIA_MIN,TIBIA_MAX,TIBIA_CALIBRATE};
  88 + femur={FEMUR_MIN,FEMUR_MAX,FEMUR_CALIBRATE};
  89 + coxa={COXA_MIN,COXA_MAX,COXA_CALIBRATE};
  90 +
  91 + int numPin=0; //assigne les pins
  92 + for(int i=0; i<6; i++)
  93 + {
  94 + for(int j=0; j<3; j++)
  95 + {
  96 + patte[i][j]=numPin;
  97 + numPin++;
  98 + }
  99 + patte[5][1]=100;
  100 + patte[5][2]=100;
  101 + }
  102 +delay(10);
  103 +}
  104 +
  105 +
  106 +// Permet de convertir les degrés en pulselen
  107 +int degToPuls(int degrees, struct legPart leg) {
  108 + return(map(degrees, 0, 180, leg.legMin, leg.legMax));
  109 +}
  110 +// Permet de convertir les pulselen en degrés
  111 +int pulsToDeg(int pulselen, struct legPart leg) {
  112 + return(map(pulselen, leg.legMin, leg.legMax, 0, 180));
  113 +}
  114 +
  115 +/*
  116 +========================================
  117 +Fonctions de contôle des servos moteurs
  118 +========================================
  119 +*/
  120 +
  121 +//commander une patte
  122 +void mouvPatte(int patteNum, int tibiaPuls, int femurPuls, int coxaPuls){
  123 +
  124 + pwm.setPWM(patte[patteNum][0],0, tibiaPuls);
  125 +
  126 + if(patte[patteNum][1]==100)
  127 + {servo16.write(pulsToDeg(femurPuls,femur));}
  128 + else
  129 + {pwm.setPWM(patte[patteNum][1],0,femurPuls);}
  130 +
  131 + if(patte[patteNum][2]==100)
  132 + {servo17.write(pulsToDeg(coxaPuls,coxa));}
  133 + else
  134 + {pwm.setPWM(patte[patteNum][2],0, coxaPuls);}
  135 +}
  136 +
  137 +
  138 +// Mettre toutes les pattes à la même position
  139 +void all(int tibiaPuls, int femurPuls, int coxaPuls){
  140 + for(int i=0; i<6; i++){
  141 + mouvPatte(i, tibiaPuls, femurPuls, coxaPuls);
  142 + }
  143 +}
  144 +
  145 +/*
  146 + ==================
  147 + Mouvements de base
  148 + ==================
  149 + */
  150 +
  151 +void tourneGauche(int vitesse){
  152 + mouvPatte(1,TIBIA_CALIBRATE,350,410);
  153 + mouvPatte(4,TIBIA_CALIBRATE,350,410);
  154 + delay(vitesse);
  155 + mouvPatte(1,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
  156 + mouvPatte(4,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
  157 + delay(vitesse);
  158 + mouvPatte(2,TIBIA_CALIBRATE,350,410);
  159 + mouvPatte(5,TIBIA_CALIBRATE,350,410);
  160 + delay(vitesse);
  161 + mouvPatte(2,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
  162 + mouvPatte(5,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
  163 + delay(vitesse);
  164 + mouvPatte(0,TIBIA_CALIBRATE,350,410);
  165 + mouvPatte(3,TIBIA_CALIBRATE,350,410);
  166 + delay(vitesse);
  167 + mouvPatte(0,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
  168 + mouvPatte(3,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
  169 + delay(vitesse);
  170 + all(TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
  171 + delay(vitesse);
  172 +}
  173 +
  174 +void tourneDroite(int vitesse){
  175 + mouvPatte(1,TIBIA_CALIBRATE,350,230);
  176 + mouvPatte(4,TIBIA_CALIBRATE,350,230);
  177 + delay(vitesse);
  178 + mouvPatte(1,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
  179 + mouvPatte(4,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
  180 + delay(vitesse);
  181 + mouvPatte(2,TIBIA_CALIBRATE,350,230);
  182 + mouvPatte(5,TIBIA_CALIBRATE,350,230);
  183 + delay(vitesse);
  184 + mouvPatte(2,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
  185 + mouvPatte(5,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
  186 + delay(vitesse);
  187 + mouvPatte(0,TIBIA_CALIBRATE,350,230);
  188 + mouvPatte(3,TIBIA_CALIBRATE,350,230);
  189 + delay(vitesse);
  190 + mouvPatte(0,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
  191 + mouvPatte(3,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
  192 + delay(vitesse);
  193 + all(TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
  194 + delay(vitesse);
  195 +}
  196 +
  197 +void lever(void){
  198 + all(TIBIA_MIN,FEMUR_MIN,COXA_CALIBRATE);
  199 + delay(200);
  200 + all(TIBIA_MIN,250,COXA_CALIBRATE);
  201 + delay(200);
  202 + all(TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
  203 + delay(500);
  204 +}
  205 +
  206 +void faireUnPas(int mode, int vitesse) //mode = direction
  207 +{
  208 +mouvPatte((2+mode)%6,TIBIA_CALIBRATE, 350, 230);
  209 + delay(vitesse);
  210 + mouvPatte((2+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, 230);
  211 + delay(vitesse);
  212 + mouvPatte((4+mode)%6,TIBIA_CALIBRATE, 350, 410);
  213 + delay(vitesse);
  214 + mouvPatte((4+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, 410);
  215 + delay(vitesse);
  216 + mouvPatte((0+mode)%6,TIBIA_CALIBRATE, 350, COXA_CALIBRATE);
  217 + mouvPatte((3+mode)%6,TIBIA_CALIBRATE, 350, COXA_CALIBRATE);
  218 + delay(vitesse);
  219 + mouvPatte((2+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, COXA_CALIBRATE);
  220 + mouvPatte((4+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, COXA_CALIBRATE);
  221 + mouvPatte((1+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, 410);
  222 + mouvPatte((5+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, 230);
  223 + delay(vitesse);
  224 + mouvPatte((0+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, COXA_CALIBRATE);
  225 + mouvPatte((3+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, COXA_CALIBRATE);
  226 + delay(vitesse);
  227 + mouvPatte((1+mode)%6,TIBIA_CALIBRATE, 350, COXA_CALIBRATE);
  228 + delay(vitesse);
  229 + mouvPatte((1+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, COXA_CALIBRATE);
  230 + delay(vitesse);
  231 + mouvPatte((5+mode)%6,TIBIA_CALIBRATE, 350, COXA_CALIBRATE);
  232 + delay(vitesse);
  233 + mouvPatte((5+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, COXA_CALIBRATE);
  234 + delay(vitesse);
  235 +}
  236 +
  237 +/*
  238 + =============================
  239 + Fonctions liées aux capteurs
  240 + =============================
  241 + */
  242 +void getCapteurs()
  243 +{
  244 + digitalWrite(TRIGGER_AVANT, HIGH);
  245 + delayMicroseconds(10);
  246 + digitalWrite(TRIGGER_AVANT, LOW);
  247 + long measure_avant = pulseIn(ECHO_AVANT, HIGH, MEASURE_TIMEOUT);
  248 + cap.avant = measure_avant / 20.0 * SOUND_SPEED;
  249 +
  250 + digitalWrite(TRIGGER_DROITE, HIGH);
  251 + delayMicroseconds(10);
  252 + digitalWrite(TRIGGER_DROITE, LOW);
  253 + long measure_droite = pulseIn(ECHO_DROITE, HIGH, MEASURE_TIMEOUT);
  254 + cap.droite = measure_droite / 20.0 * SOUND_SPEED;
  255 +
  256 +
  257 + digitalWrite(TRIGGER_GAUCHE, HIGH);
  258 + delayMicroseconds(10);
  259 + digitalWrite(TRIGGER_GAUCHE, LOW);
  260 + long measure_gauche = pulseIn(ECHO_GAUCHE, HIGH, MEASURE_TIMEOUT);
  261 + cap.gauche = measure_gauche / 20.0 * SOUND_SPEED;
  262 +}
  263 +
  264 +void afficheCapteurs()
  265 +{
  266 + Serial.print("Avant : ");
  267 + Serial.println(cap.avant);
  268 + Serial.print("Droite : ");
  269 + Serial.println(cap.droite);
  270 + Serial.print("Gauche ");
  271 + Serial.println(cap.gauche);
  272 + Serial.println("==============");
  273 +}
  274 +
  275 +/*
  276 + ========================
  277 + Séquences de mouvements
  278 + ========================
  279 + */
  280 +
  281 +void marcheClassique(int vitesse)
  282 +{
  283 + getCapteurs();
  284 + if((cap.avant<20 and cap.droite< cap.gauche))
  285 + {
  286 + tourneGauche(vitesse);
  287 + delay(300);
  288 + tourneGauche(vitesse);
  289 + delay(300);
  290 + tourneGauche(vitesse);
  291 + delay(300);
  292 + tourneGauche(vitesse);
  293 + delay(300);
  294 + }
  295 + else if((cap.avant<20 and cap.droite> cap.gauche) or cap.gauche<20)
  296 + {
  297 + tourneDroite(vitesse);
  298 + delay(300);
  299 + tourneDroite(vitesse);
  300 + delay(300);
  301 + tourneDroite(vitesse);
  302 + delay(300);
  303 + tourneDroite(vitesse);
  304 + delay(300);
  305 + }
  306 + else if(cap.droite<20)
  307 + {
  308 + tourneGauche(vitesse);
  309 + }
  310 + else if(cap.gauche<20)
  311 + {
  312 + tourneDroite(vitesse);
  313 + }
  314 + else
  315 + {
  316 + faireUnPas(AVANT,vitesse);
  317 + }
  318 +}
  319 +
  320 +void longeMur(int vitesse)
  321 +{
  322 + getCapteurs();
  323 + if(cap.droite<15)
  324 + {
  325 + faireUnPas(AVANT_GAUCHE,vitesse);
  326 + }
  327 + else if(cap.droite>25)
  328 + {
  329 + faireUnPas(AVANT_DROITE,vitesse);
  330 + }
  331 + else
  332 + {
  333 + faireUnPas(AVANT,vitesse);
  334 + }
  335 +}
  336 +
  337 +void loop()
  338 +{
  339 +
  340 +marcheClassique(50);
  341 +delay(500);
  342 +
  343 +}
  344 +
Programmation/Deplacement/Deplacement.ino deleted
@@ -1,196 +0,0 @@ @@ -1,196 +0,0 @@
1 -/*  
2 -Pour que le programme fonctionne correctement les servos moteurs doivent être connecté de la façon suivante :  
3 -Pour une patte les 3 servos doivent se suivrent (ex : pin0 ,pin1, pin2)  
4 -Les tibia doivent être sur les pins 0 - 3 - 6 - 9 - 12 - 15  
5 -Les femur doivent être sur les pins 1 - 4 - 7 - 10 - 13  
6 -Les coxa doivent être sur les pins 2 - 5 - 8 - 11 - 14  
7 - */  
8 -#include <Wire.h>  
9 -#include <Servo.h>  
10 -#include <Adafruit_PWMServoDriver.h>  
11 -  
12 -Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();  
13 -Servo servo16; //femur  
14 -Servo servo17; //coxa  
15 -  
16 -#define TIBIA_MIN 150 //tibia en haut  
17 -#define TIBIA_MAX 400 //tibia en bas  
18 -#define TIBIA_CALIBRATE 220 //62°  
19 -  
20 -#define FEMUR_MIN 200 //200  
21 -#define FEMUR_MAX 490 //500  
22 -#define FEMUR_CALIBRATE 420 //115°  
23 -  
24 -#define COXA_MIN 100 //coxa à gauche  
25 -#define COXA_MAX 550 //coxa à droite  
26 -#define COXA_CALIBRATE 320 //90°  
27 -  
28 -struct legPart {  
29 - int legMin;  
30 - int legMax;  
31 - int legCalibrate;  
32 -} legPart;  
33 -  
34 -struct legPart tibia;  
35 -struct legPart femur;  
36 -struct legPart coxa;  
37 -  
38 -int patte[6][3]; //chaque patte est un tableau contenant le numero du pin pour tibia/femur/coxa. Ex : acceder au pin du gémur patte 2 -> patte[1][0]  
39 -  
40 -  
41 -void setup() {  
42 - servo16.attach(10); //femur  
43 - servo17.attach(11); //coxa  
44 - Serial.begin(9600);  
45 - pwm.begin();  
46 - pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates  
47 -  
48 - tibia={TIBIA_MIN,TIBIA_MAX,TIBIA_CALIBRATE};  
49 - femur={FEMUR_MIN,FEMUR_MAX,FEMUR_CALIBRATE};  
50 - coxa={COXA_MIN,COXA_MAX,COXA_CALIBRATE};  
51 -  
52 - int numPin=0; //assigne les pins  
53 - for(int i=0; i<6; i++){  
54 - for(int j=0; j<3; j++){  
55 - patte[i][j]=numPin;  
56 - numPin++;  
57 - }  
58 - patte[5][1]=100;  
59 - patte[5][2]=100;  
60 - }  
61 - delay(10);  
62 -}  
63 -  
64 -  
65 -// Permet de convertir les degrés en pulselen  
66 -int degToPuls(int degrees, struct legPart leg) {  
67 - return(map(degrees, 0, 180, leg.legMin, leg.legMax));  
68 -}  
69 -// Permet de convertir les pulselen en degrés  
70 -int pulsToDeg(int pulselen, struct legPart leg) {  
71 - return(map(pulselen, leg.legMin, leg.legMax, 0, 180));  
72 -}  
73 -  
74 -//commander une patte  
75 -void mouvPatte(int patteNum, int tibiaPuls, int femurPuls, int coxaPuls){  
76 -  
77 - pwm.setPWM(patte[patteNum][0],0, tibiaPuls);  
78 -  
79 - if(patte[patteNum][1]==100)  
80 - {servo16.write(pulsToDeg(femurPuls,femur));}  
81 - else  
82 - {pwm.setPWM(patte[patteNum][1],0,femurPuls);}  
83 -  
84 - if(patte[patteNum][2]==100)  
85 - {servo17.write(pulsToDeg(coxaPuls,coxa));}  
86 - else  
87 - {pwm.setPWM(patte[patteNum][2],0, coxaPuls);}  
88 -}  
89 -  
90 -  
91 -// Mettre toutes les pattes à la même position  
92 -void all(int tibiaPuls, int femurPuls, int coxaPuls){  
93 - for(int i=0; i<6; i++){  
94 - mouvPatte(i, tibiaPuls, femurPuls, coxaPuls);  
95 - }  
96 -}  
97 -  
98 -void marcheAvant(int vitesse){  
99 - mouvPatte(5,TIBIA_CALIBRATE,350,COXA_CALIBRATE);  
100 - delay(vitesse);  
101 - mouvPatte(5,TIBIA_CALIBRATE,350,410);  
102 - delay(vitesse);  
103 - mouvPatte(5,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);  
104 - delay(vitesse);  
105 - mouvPatte(2,TIBIA_CALIBRATE,350,COXA_CALIBRATE);  
106 - delay(vitesse);  
107 - mouvPatte(2,TIBIA_CALIBRATE,350,230);  
108 - delay(vitesse);  
109 - mouvPatte(2,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);  
110 - delay(vitesse);  
111 - mouvPatte(0,TIBIA_CALIBRATE,350,COXA_CALIBRATE);  
112 - mouvPatte(3,TIBIA_CALIBRATE,350,COXA_CALIBRATE);  
113 - delay(vitesse);  
114 - mouvPatte(5,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);  
115 - mouvPatte(1,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);  
116 - mouvPatte(4,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);  
117 - mouvPatte(2,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);  
118 - delay(vitesse);  
119 - mouvPatte(0,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);  
120 - mouvPatte(3,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);  
121 - delay(vitesse);  
122 - mouvPatte(1,TIBIA_CALIBRATE,350,410);  
123 - delay(vitesse);  
124 - mouvPatte(1,TIBIA_CALIBRATE,350,COXA_CALIBRATE);  
125 - delay(vitesse);  
126 - mouvPatte(1,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);  
127 - delay(vitesse);  
128 - mouvPatte(4,TIBIA_CALIBRATE,350,230);  
129 - delay(vitesse);  
130 - mouvPatte(4,TIBIA_CALIBRATE,350,COXA_CALIBRATE);  
131 - delay(vitesse);  
132 - mouvPatte(4,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);  
133 - delay(vitesse);  
134 -}  
135 -  
136 -void marcheArriere(int vitesse){  
137 - mouvPatte(2,TIBIA_CALIBRATE,350,COXA_CALIBRATE);  
138 - delay(vitesse);  
139 - mouvPatte(2,TIBIA_CALIBRATE,350,410);  
140 - delay(vitesse);  
141 - mouvPatte(2,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);  
142 - delay(vitesse);  
143 - mouvPatte(5,TIBIA_CALIBRATE,350,COXA_CALIBRATE);  
144 - delay(vitesse);  
145 - mouvPatte(5,TIBIA_CALIBRATE,350,230);  
146 - delay(vitesse);  
147 - mouvPatte(5,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);  
148 - delay(vitesse);  
149 - mouvPatte(3,TIBIA_CALIBRATE,350,COXA_CALIBRATE);  
150 - mouvPatte(0,TIBIA_CALIBRATE,350,COXA_CALIBRATE);  
151 - delay(vitesse);  
152 - mouvPatte(2,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);  
153 - mouvPatte(4,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);  
154 - mouvPatte(1,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);  
155 - mouvPatte(5,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);  
156 - delay(vitesse);  
157 - mouvPatte(3,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);  
158 - mouvPatte(0,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);  
159 - delay(vitesse);  
160 - mouvPatte(4,TIBIA_CALIBRATE,350,410);  
161 - delay(vitesse);  
162 - mouvPatte(4,TIBIA_CALIBRATE,350,COXA_CALIBRATE);  
163 - delay(vitesse);  
164 - mouvPatte(4,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);  
165 - delay(vitesse);  
166 - mouvPatte(1,TIBIA_CALIBRATE,350,230);  
167 - delay(vitesse);  
168 - mouvPatte(1,TIBIA_CALIBRATE,350,COXA_CALIBRATE);  
169 - delay(vitesse);  
170 - mouvPatte(1,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);  
171 - delay(vitesse);  
172 -}  
173 -  
174 -void tourneGauche(int vitesse){  
175 - //mouvPatte();  
176 -}  
177 -  
178 -void lever(void){  
179 - all(TIBIA_MIN,FEMUR_MIN,COXA_CALIBRATE);  
180 - delay(200);  
181 - all(TIBIA_MIN,250,COXA_CALIBRATE);  
182 - delay(200);  
183 - all(TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);  
184 - delay(500);  
185 -}  
186 -void loop() {  
187 - all(TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);  
188 - delay(3000);  
189 - marcheAvant(100);  
190 - marcheAvant(100);  
191 - marcheAvant(100);  
192 - marcheAvant(100);  
193 - marcheAvant(100);  
194 - marcheAvant(100);  
195 - delay(6000);  
196 -}  
Programmation/Distance/Distance.ino deleted
@@ -1,52 +0,0 @@ @@ -1,52 +0,0 @@
1 -/*  
2 - * Code d'exemple pour un capteur à ultrasons HC-SR04.  
3 - */  
4 -  
5 -/* Constantes pour les broches */  
6 -const byte TRIGGER_PIN = 9; // Broche TRIGGER  
7 -const byte ECHO_PIN = 8; // Broche ECHO  
8 -  
9 -/* Constantes pour le timeout */  
10 -const unsigned long MEASURE_TIMEOUT = 25000UL; // 25ms = ~8m à 340m/s  
11 -  
12 -/* Vitesse du son dans l'air en mm/us */  
13 -const float SOUND_SPEED = 340.0 / 1000;  
14 -  
15 -/** Fonction setup() */  
16 -void setup() {  
17 -  
18 - /* Initialise le port série */  
19 - Serial.begin(115200);  
20 -  
21 - /* Initialise les broches */  
22 - pinMode(TRIGGER_PIN, OUTPUT);  
23 - digitalWrite(TRIGGER_PIN, LOW); // La broche TRIGGER doit être à LOW au repos  
24 - pinMode(ECHO_PIN, INPUT);  
25 -}  
26 -  
27 -/** Fonction loop() */  
28 -void loop() {  
29 -  
30 - /* 1. Lance une mesure de distance en envoyant une impulsion HIGH de 10µs sur la broche TRIGGER */  
31 - digitalWrite(TRIGGER_PIN, HIGH);  
32 - delayMicroseconds(10);  
33 - digitalWrite(TRIGGER_PIN, LOW);  
34 -  
35 - /* 2. Mesure le temps entre l'envoi de l'impulsion ultrasonique et son écho (si il existe) */  
36 - long measure = pulseIn(ECHO_PIN, HIGH, MEASURE_TIMEOUT);  
37 -  
38 - /* 3. Calcul la distance à partir du temps mesuré */  
39 - float distance_mm = measure / 2.0 * SOUND_SPEED;  
40 -  
41 - /* Affiche les résultats en mm, cm et m */  
42 - Serial.print("Distance: ");  
43 - Serial.print(distance_mm);  
44 - Serial.print("mm (");  
45 - Serial.print(distance_mm / 10.0, 2);  
46 - Serial.print("cm, ");  
47 - Serial.print(distance_mm / 1000.0, 2);  
48 - Serial.println("m)");  
49 -  
50 - /* Délai d'attente pour éviter d'afficher trop de résultats à la seconde */  
51 - delay(500);  
52 -}  
Programmation/RSSI.ino 0 → 100644
@@ -0,0 +1,116 @@ @@ -0,0 +1,116 @@
  1 +/*
  2 + WriteSingleField
  3 +
  4 + Description: Writes a value to a channel on ThingSpeak every 20 seconds.
  5 +
  6 + Hardware: Arduino compatible hardware controlling ESP8266 through AT commands. THIS IS CODE FOR THE ARDUINO, NOT THE ESP8266!
  7 +
  8 + !!! IMPORTANT - Modify the secrets.h file for this project with your network connection and ThingSpeak channel details. !!!
  9 +
  10 + Note:
  11 + - Requires Arduino main board connected to ESP8266 (ESP-01) serial pins
  12 + - The ESP8266 device must be running firmware capable of AT commands over TX/RX pins.
  13 + Details on reflashing and binaries can be found here: https://www.espressif.com/en/support/download/at
  14 + - Requires WiFiEsp library which is available through the Library Manager. The logging level for the WiFiEsp library is set to INFO.
  15 + To disable logging completely, set _ESPLOGLEVEL_ to 0 in \Arduino\libraries\WiFiEsp\src\utility\debug.h
  16 + - Use TX1/RX1 if present on Arduino header (Mega, Due, etc). If not, then connect TX to pin 7, RX to pin 6.
  17 + - Some boards (Uno, Nano, Leonardo, etc) do not have enough memory to handle writing large strings to ThingSpeak.
  18 + For these boards, keep individual field data lengths 32 characters or less.
  19 + - This example is written for a network using WPA encryption. For WEP or WPA, change the WiFi.begin() call accordingly.
  20 +
  21 + Wiring diagrams are available at:
  22 + SoftSerial (Uno, Nano, Mini, etc): https://github.com/mathworks/thingspeak-arduino/blob/master/ESP-01_AT_Commands_SoftSerial_Hookup.pdf
  23 + Hardware Serial1 (Mega, Leonardo, Due) - https://github.com/mathworks/thingspeak-arduino/blob/master/ESP-01_AT_Commands_Hardware_Serial_Hookup.pdf
  24 +
  25 + ESP8266 | Arduino without Serial1 | Arduino with Serial1
  26 + --------------------------------------------------------
  27 + RX | pin 7 | TX1
  28 + TX | pin 6 | RX1
  29 + GND | GND | GND
  30 + VCC | 5V | 5V
  31 + CH_PD | 5V | 5V
  32 +
  33 + ThingSpeak ( https://www.thingspeak.com ) is an analytic IoT platform service that allows you to aggregate, visualize, and
  34 + analyze live data streams in the cloud. Visit https://www.thingspeak.com to sign up for a free account and create a channel.
  35 +
  36 + Documentation for the ThingSpeak Communication Library for Arduino is in the README.md folder where the library was installed.
  37 + See https://www.mathworks.com/help/thingspeak/index.html for the full ThingSpeak documentation.
  38 +
  39 + For licensing information, see the accompanying license file.
  40 +
  41 + Copyright 2019, The MathWorks, Inc.
  42 +*/
  43 +
  44 +
  45 +
  46 +#include "WiFiEsp.h"
  47 +
  48 +char ssid[] = SECRET_SSID; // your network SSID (name)
  49 +char pass[] = SECRET_PASS; // your network password
  50 +
  51 +WiFiEspClient client;
  52 +
  53 +#include "SoftwareSerial.h"
  54 +SoftwareSerial Serial1(6, 7);
  55 +#define ESP_BAUDRATE 115200
  56 +
  57 +
  58 +void setup() {
  59 + //Initialize serial and wait for port to open
  60 + Serial.begin(9600);
  61 +
  62 + // initialize serial for ESP module
  63 + setEspBaudRate(ESP_BAUDRATE);
  64 +
  65 + Serial.print("Searching for ESP8266...");
  66 + // initialize ESP module
  67 + WiFi.init(&Serial1);
  68 +
  69 +}
  70 +
  71 +void loop() {
  72 + mesureRSSI();
  73 + delay(5000);
  74 +}
  75 +
  76 +// This function attempts to set the ESP8266 baudrate. Boards with additional hardware serial ports
  77 +// can use 115200, otherwise software serial is limited to 19200.
  78 +void setEspBaudRate(unsigned long baudrate){
  79 + long rates[6] = {115200,74880,57600,38400,19200,9600};
  80 +
  81 + Serial.print("Setting ESP8266 baudrate to ");
  82 + Serial.print(baudrate);
  83 + Serial.println("...");
  84 +
  85 + for(int i = 0; i < 6; i++){
  86 + Serial1.begin(rates[i]);
  87 + delay(100);
  88 + Serial1.print("AT+UART_DEF=");
  89 + Serial1.print(baudrate);
  90 + Serial1.print(",8,1,0,0\r\n");
  91 + delay(100);
  92 + }
  93 + Serial1.begin(baudrate);
  94 +}
  95 +
  96 +void mesureRSSI(){
  97 + Serial.println("Prise de mesure\n");
  98 + // Connect or reconnect to WiFi
  99 + if(WiFi.status() != WL_CONNECTED){
  100 + //Serial.print("Attempting to connect to SSID: ");
  101 + //Serial.println(SECRET_SSID);
  102 + while(WiFi.status() != WL_CONNECTED){
  103 + WiFi.begin(ssid, pass); // Connect to WPA/WPA2 network. Change this line if using open or WEP network
  104 + //Serial.print(".");
  105 + delay(100);
  106 + }
  107 + //Serial.println("\nConnected");
  108 + }
  109 +
  110 + int rssi = 0;
  111 + while(rssi==0){
  112 + rssi = WiFi.RSSI();
  113 + }
  114 + Serial.print("RSSI : ");
  115 + Serial.println(rssi);
  116 +}