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
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
|
/*
LABO moteur X LED KCD3_SPDT
*/
/*
Arduino Mega + A4988
Capteurs (IR en U) + potentiomètre contrôle distance et direction après recul
--- Diagramme d’état : Moteur + Potar + Capteurs ---
[START / Moteur immobile]
|
| Potar bougé → au-delà zone morte
v
[Déplacement par potar]
|
| Capteur déclenché (IR)
v
[Recul 1 cm en micro-pas]
|
| Recul terminé
v
[Moteur en PAUSE après recul]
|
| Potar manipulé par l'utilisateur (delta > zone morte)
v
[Reprise contrôle par potar]
|
+--> [Déplacement par potar]
*/
// Moteur PIN
const int MOTOR_DIR_PIN = 2;
const int MOTOR_STEP_PIN = 3;
const int MOTOR_ENABLE_PIN = 6;
const int MOTOR_ENABLE_PIN_Z = 13;
// EndStop PIN signal
const int ENDSTOP_IR0 = 7;
const int ENDSTOP_IR1 = 11;
const int ENDSTOP_IR0_Z = 4;
const int ENDSTOP_IR1_Z = 5;
// EndStop MSx PIN
const int MS1_PIN = 8;
const int MS2_PIN = 9;
const int MS3_PIN = 10;
// Potentiomètre PIN Signal
const int POT_PIN = A0;
// Interupteur KCD3SPDT ON-OFF-ON PIN Signal
const int KCD3_SPDT_PIN_NO_D = A6;
const int KCD3_SPDT_PIN_NO_G = A5;
// LEDs
const int ledPin_Green_X = 12; // LED Green X
const int ledPin_Green_Z = A8; // LED Green Z
// Paramètres moteur
const int Nb_Recul = 200; // 1cm en full-step=400pas --- 0.5cm en full-step=200pas
const int microStepFactor = 16; // micro-pas pour recul
/*
Le moteur avance d’un pas à chaque impulsion STEP.
La vitesse du moteur dépend uniquement du temps entre deux impulsions.
motorDelayMax est grand, plus le mouvement minimal est lent et précis.
Plus motorDelayMin est petit, plus le moteur va vite.
*/
const int motorDelayMax = 1500; // microsecondes max STEP µs (lent) 1500 ≈ 1 mm/s
const int motorDelayMin = 400; // microsecondes min STEP µs (rapide) 400 ≈ 3,9 mm/s
/*
La zone morte (deadZone) est un intervalle central autour de la valeur neutre du potar où aucun mouvement du moteur n’est déclenché.
Exemple : const int deadZone = 50;
Si le potentiomètre est entre -50 et +50 autour du centre, le moteur reste immobile.
*/
const int deadZone = 200; // zone morte du potar
const int accelStep = 1; // incrément de lissage pour acceleration 2 4
bool reculEffectue = false; // vrai après recul
bool pauseAfterRecul = false; // moteur en pause après recul
int potRef = 0; // valeur de référence du potar après recul
int ir0LastState = 0;
int ir1LastState = 0;
int currentDelay = motorDelayMax;
bool potInverted = false; // true = inverser la direction du potar
// Fonction STEP simple
void stepMotor(int delayMicro) {
digitalWrite(MOTOR_STEP_PIN, HIGH);
delayMicroseconds(delayMicro);
digitalWrite(MOTOR_STEP_PIN, LOW);
delayMicroseconds(delayMicro);
}
void setup() {
// LEDs
pinMode(ledPin_Green_X, OUTPUT);
pinMode(ledPin_Green_Z, OUTPUT);
// KCD3
pinMode(KCD3_SPDT_PIN_NO_D, INPUT_PULLUP);
pinMode(KCD3_SPDT_PIN_NO_G, INPUT_PULLUP);
// Moteurs
pinMode(MOTOR_DIR_PIN, OUTPUT);
pinMode(MOTOR_STEP_PIN, OUTPUT);
pinMode(MOTOR_ENABLE_PIN, OUTPUT);
pinMode(MOTOR_ENABLE_PIN_Z, OUTPUT);
// pinMode(ENDSTOP_MECA, INPUT_PULLUP);
pinMode(ENDSTOP_IR0, INPUT);
pinMode(ENDSTOP_IR1, INPUT);
pinMode(ENDSTOP_IR0_Z, INPUT);
pinMode(ENDSTOP_IR1_Z, INPUT);
pinMode(MS1_PIN, OUTPUT);
pinMode(MS2_PIN, OUTPUT);
pinMode(MS3_PIN, OUTPUT);
// Micro-pas 1/16 pour recul
digitalWrite(MS1_PIN, HIGH);
digitalWrite(MS2_PIN, HIGH);
digitalWrite(MS3_PIN, HIGH);
//digitalWrite(MOTOR_ENABLE_PIN, LOW);
// LED Etat OFF
digitalWrite(ledPin_Green_X, LOW);
digitalWrite(ledPin_Green_Z, LOW);
delay(100); // stabilisation capteurs
ir0LastState = digitalRead(ENDSTOP_IR0);
ir1LastState = digitalRead(ENDSTOP_IR1);
}
void loop() {
int etatNO_D = digitalRead(KCD3_SPDT_PIN_NO_D); // HIGH = ouvert, LOW = fermé
int etatNO_G = digitalRead(KCD3_SPDT_PIN_NO_G);
if (etatNO_D == HIGH && etatNO_G == HIGH){
// LED OFF
digitalWrite(ledPin_Green_X, LOW);
digitalWrite(ledPin_Green_Z, LOW);
}
if (etatNO_G == LOW){
// LED OFF
digitalWrite(ledPin_Green_X, LOW);
digitalWrite(ledPin_Green_Z, HIGH);
}
if (etatNO_D == LOW){
// LED ON
digitalWrite(ledPin_Green_X, HIGH);
digitalWrite(ledPin_Green_Z, LOW);
// ===== Lecture potar =====
int potValue = analogRead(POT_PIN) - 512;
int potAbs = abs(potValue);
// Détermination direction potar avec inversion si activé
bool direction = potInverted ? (potValue < 0) : (potValue > 0);
// ===== Lecture capteurs =====
int ir0State = digitalRead(ENDSTOP_IR0);
int ir1State = digitalRead(ENDSTOP_IR1);
bool ir0Triggered = (ir0State == HIGH && ir0LastState == LOW);
bool ir1Triggered = (ir1State == HIGH && ir1LastState == LOW);
// ===== Recul 1 cm si capteur déclenché =====
if(!reculEffectue && (ir0Triggered || ir1Triggered)){
delay(50);
// Micro-pas pour recul
digitalWrite(MS1_PIN, HIGH);
digitalWrite(MS2_PIN, HIGH);
digitalWrite(MS3_PIN, HIGH);
digitalWrite(MOTOR_DIR_PIN, !direction); // reculer dans sens opposé au potar
for(int i=0; i<Nb_Recul*microStepFactor; i++){
stepMotor(motorDelayMin);
}
// Recul terminé → moteur en pause
reculEffectue = true;
pauseAfterRecul = true;
potRef = potValue; // mémoriser la valeur du potar après recul
// Mise à jour états capteurs
ir0LastState = digitalRead(ENDSTOP_IR0);
ir1LastState = digitalRead(ENDSTOP_IR1);
return; // ne rien faire d'autre
}
// ===== Vérification pause après recul =====
if(pauseAfterRecul){
int deltaPot = abs(potValue - potRef);
if(deltaPot > deadZone){
// l'utilisateur a bougé le potar → reprise
pauseAfterRecul = false;
} else {
return; // moteur reste en pause
}
}
// ===== STEP proportionnel au potar avec acceleration =====
if(potAbs >= deadZone){
// Micro-pas 1/16 pour recul
digitalWrite(MS1_PIN, HIGH);
digitalWrite(MS2_PIN, HIGH);
digitalWrite(MS3_PIN, HIGH);
/*
// Passer en full-step pour potar
digitalWrite(MS1_PIN, LOW);
digitalWrite(MS2_PIN, LOW);
digitalWrite(MS3_PIN, LOW);
*/
digitalWrite(MOTOR_DIR_PIN, direction);
// Calcul vitesse cible selon potar
int targetDelay = map(potAbs, deadZone, 512, motorDelayMax, motorDelayMin);
targetDelay = constrain(targetDelay, motorDelayMin, motorDelayMax);
// Lissage acceleration/déceleration
if(currentDelay < targetDelay)
currentDelay += accelStep;
else if(currentDelay > targetDelay)
currentDelay -= accelStep;
currentDelay = constrain(currentDelay, motorDelayMin, motorDelayMax);
stepMotor(currentDelay);
// Autoriser un nouveau recul si le potar est utilisé après recul
if(reculEffectue){
reculEffectue = false;
}
}
// Mise à jour derniers états des capteurs
ir0LastState = ir0State;
ir1LastState = ir1State;
}
} |
Travelling X Z pour Raspberry Pi Camera Module 3 via arduino mega 2560
Répondre avec citation
Envoyé par LS_Soleil
Partager