From 7ed69e859b8c3c084539edd2227c66c4fea4c60c Mon Sep 17 00:00:00 2001 From: rfoucaul Date: Fri, 19 Apr 2019 21:43:16 +0200 Subject: [PATCH] Ajout de la première version de programmation du déplacement --- Programmation/Deplacement/Deplacement.ino | 196 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 196 insertions(+), 0 deletions(-) create mode 100644 Programmation/Deplacement/Deplacement.ino diff --git a/Programmation/Deplacement/Deplacement.ino b/Programmation/Deplacement/Deplacement.ino new file mode 100644 index 0000000..0afa45f --- /dev/null +++ b/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 +#include +#include + +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); +} -- libgit2 0.21.2