Ajout de la première version de programmation du déplacement

rfoucaul
1 parent 5f116c50
Showing 1 changed file with 196 additions and 0 deletions Show diff stats
Programmation/Deplacement/Deplacement.ino
@@ -0,0 +1,196 @@
+/*
+Pour que le programme fonctionne correctement les servos moteurs doivent être connecté de la façon suivante :
+Pour une patte les 3 servos doivent se suivrent (ex : pin0 ,pin1, pin2)
+Les tibia doivent être sur les pins 0 - 3 - 6 - 9 - 12 - 15
+Les femur doivent être sur les pins 1 - 4 - 7 - 10 - 13
+Les coxa doivent être sur les pins 2 - 5 - 8 - 11 - 14 
+ */
+#include <Wire.h>
+#include <Servo.h>
+#include <Adafruit_PWMServoDriver.h>
+
+Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
+Servo servo16; //femur
+Servo servo17; //coxa
+
+#define TIBIA_MIN  150 //tibia en haut
+#define TIBIA_MAX  400 //tibia en bas
+#define TIBIA_CALIBRATE 220 //62°
+
+#define FEMUR_MIN  200 //200
+#define FEMUR_MAX  490 //500
+#define FEMUR_CALIBRATE 420 //115°
+
+#define COXA_MIN  100 //coxa à gauche
+#define COXA_MAX  550 //coxa à droite
+#define COXA_CALIBRATE 320 //90°
+
+struct legPart {
+  int legMin;
+  int legMax;
+  int legCalibrate;
+} legPart;
+
+struct legPart tibia;
+struct legPart femur;
+struct legPart coxa;
+
+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]
+
+
+void setup() {
+  servo16.attach(10); //femur
+  servo17.attach(11); //coxa
+  Serial.begin(9600);
+  pwm.begin();
+  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
+  
+  tibia={TIBIA_MIN,TIBIA_MAX,TIBIA_CALIBRATE};
+  femur={FEMUR_MIN,FEMUR_MAX,FEMUR_CALIBRATE};
+  coxa={COXA_MIN,COXA_MAX,COXA_CALIBRATE};
+  
+  int numPin=0; //assigne les pins
+  for(int i=0; i<6; i++){
+    for(int j=0; j<3; j++){
+      patte[i][j]=numPin;
+      numPin++;
+      }
+      patte[5][1]=100;
+      patte[5][2]=100;
+    }
+  delay(10);
+}
+
+
+// Permet de convertir les degrés en pulselen
+int degToPuls(int degrees, struct legPart leg) {
+  return(map(degrees, 0, 180, leg.legMin, leg.legMax));
+}
+// Permet de convertir les pulselen en degrés
+int pulsToDeg(int pulselen, struct legPart leg) {
+  return(map(pulselen, leg.legMin, leg.legMax, 0, 180));
+}
+
+//commander une patte
+void mouvPatte(int patteNum, int tibiaPuls, int femurPuls, int coxaPuls){
+    
+    pwm.setPWM(patte[patteNum][0],0, tibiaPuls);
+    
+    if(patte[patteNum][1]==100)
+      {servo16.write(pulsToDeg(femurPuls,femur));}
+    else
+      {pwm.setPWM(patte[patteNum][1],0,femurPuls);}
+    
+    if(patte[patteNum][2]==100)
+      {servo17.write(pulsToDeg(coxaPuls,coxa));}
+    else
+      {pwm.setPWM(patte[patteNum][2],0, coxaPuls);}
+}
+
+
+// Mettre toutes les pattes à la même position
+void all(int tibiaPuls, int femurPuls, int coxaPuls){ 
+  for(int i=0; i<6; i++){
+    mouvPatte(i, tibiaPuls, femurPuls, coxaPuls);
+    }
+}
+
+void marcheAvant(int vitesse){
+  mouvPatte(5,TIBIA_CALIBRATE,350,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(5,TIBIA_CALIBRATE,350,410);
+  delay(vitesse);
+  mouvPatte(5,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
+  delay(vitesse);
+  mouvPatte(2,TIBIA_CALIBRATE,350,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(2,TIBIA_CALIBRATE,350,230);
+  delay(vitesse);
+  mouvPatte(2,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
+  delay(vitesse);
+  mouvPatte(0,TIBIA_CALIBRATE,350,COXA_CALIBRATE);
+  mouvPatte(3,TIBIA_CALIBRATE,350,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(5,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
+  mouvPatte(1,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
+  mouvPatte(4,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
+  mouvPatte(2,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(0,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
+  mouvPatte(3,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(1,TIBIA_CALIBRATE,350,410);
+  delay(vitesse);
+  mouvPatte(1,TIBIA_CALIBRATE,350,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(1,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(4,TIBIA_CALIBRATE,350,230);
+  delay(vitesse);
+  mouvPatte(4,TIBIA_CALIBRATE,350,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(4,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
+  delay(vitesse);
+}
+
+void marcheArriere(int vitesse){
+  mouvPatte(2,TIBIA_CALIBRATE,350,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(2,TIBIA_CALIBRATE,350,410);
+  delay(vitesse);
+  mouvPatte(2,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
+  delay(vitesse);
+  mouvPatte(5,TIBIA_CALIBRATE,350,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(5,TIBIA_CALIBRATE,350,230);
+  delay(vitesse);
+  mouvPatte(5,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
+  delay(vitesse);
+  mouvPatte(3,TIBIA_CALIBRATE,350,COXA_CALIBRATE);
+  mouvPatte(0,TIBIA_CALIBRATE,350,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(2,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
+  mouvPatte(4,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
+  mouvPatte(1,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
+  mouvPatte(5,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(3,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
+  mouvPatte(0,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(4,TIBIA_CALIBRATE,350,410);
+  delay(vitesse);
+  mouvPatte(4,TIBIA_CALIBRATE,350,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(4,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(1,TIBIA_CALIBRATE,350,230);
+  delay(vitesse);
+  mouvPatte(1,TIBIA_CALIBRATE,350,COXA_CALIBRATE);
+  delay(vitesse);
+  mouvPatte(1,TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
+  delay(vitesse);
+}
+
+void tourneGauche(int vitesse){
+  //mouvPatte();  
+}
+
+void lever(void){
+  all(TIBIA_MIN,FEMUR_MIN,COXA_CALIBRATE);
+  delay(200);
+  all(TIBIA_MIN,250,COXA_CALIBRATE);
+  delay(200);
+  all(TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
+  delay(500);
+}
+void loop() {
+  all(TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
+  delay(5000);
+  marcheAvant(100);
+  marcheAvant(100);
+  marcheAvant(100);
+  marcheArriere(100);
+  marcheArriere(100);
+  marcheArriere(100);
+  delay(5000);
+}