Commit 7ed69e859b8c3c084539edd2227c66c4fea4c60c

Authored by rfoucaul
1 parent 5f116c50

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

Showing 1 changed file with 196 additions and 0 deletions   Show diff stats
Programmation/Deplacement/Deplacement.ino 0 → 100644
@@ -0,0 +1,196 @@ @@ -0,0 +1,196 @@
  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(5000);
  189 + marcheAvant(100);
  190 + marcheAvant(100);
  191 + marcheAvant(100);
  192 + marcheArriere(100);
  193 + marcheArriere(100);
  194 + marcheArriere(100);
  195 + delay(5000);
  196 +}