rfoucaul / Projet_IMA4_P31

Blame view

Programmation/Deplacement/Deplacement.ino 5.69 KB
Edit Raw Normal View History
7ed69e85   rfoucaul   Ajout de la premi... 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
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);
  }