8 Degrees Of Freedom (DOF) Robot Using Arduino Uno
Introduction
This project is an 8-Degrees-Of-Freedom Robot. It is called 8-DOF-Robot because it has 8 servo motors. It has 2 servos for the left and right ankle, 2 servos for the left and right knee, 2 servos for the left and right hips, and 2 servos for the left and right shoulder. No movement for the shoulders yet because I am focus with the walking.
This project is my long and forgotten project because of its complexities like:
- How to control multiple servo motors at the same time?
- How to rotate servo motors in staggered timing?
- How to control it wirelessly?
- How to make it walk?
To solve the complexities, the following are what I did.
1: How to control multiple servo motors at the same time? I used what they call PROTOTHREADING which is basically the BlinkWithoutDelay in the Examples of Arduino IDE. With protothreading, it seems like the microcontroller is doing multiple task at the same time. Task like checking the command from wireless PS2, setting the command, setting the requested angle of each servo motors, rotating the servo motors, updating the LCD display, and updating the serial for debugging purposes.
2: How to rotate servo motors in staggered timing? I used what they call STATE MACHINE which is tracking the current angle of the servo versus the requested angle for that specific servo motors and rotating it gradually. I also added a protection check on each side of the servo motors to prevent it from over-turning. This is achieve by comparing the current angle versus the minimum and maximum angle possible.
3: How to control it wirelessly? I achieve this by using a wireless PS2 controller. The PS2 receiver is connected to the microcontroller then I use the Bill Porters Arduino library for PS2.
4: How to make it walk? This is the hardest part because I cannot find a sample code online to follow. There is a research paper I found but the angle mentioned are too ideal and when followed will results to robot falling down. So what I do is by trial and error in simulating how human walks. I used a four-stage per step walking (1) lean to the LEFT, (2) RIGHT foot forward up, (3) RIGHT foot forward down, and (4) balance foot with the RIGHT. This is also the same with the other side.
Atlast, after many hours of tinkering and code troubleshooting, I am able to make it walk.
- By pressing the LEFT arrow button: 1-step forward (left foot first).
- By pressing the RIGHT arrow button: 1-step forward (right foot first).
- By pressing the UP arrow button: Walking forward.
- By pressing the DOWN arrow button: Stop walking.
- By pressing the R1 + LEFT arrow button: 1-step backward (left foot first).
- By pressing the R1 + RIGHT arrow button: 1-step backward (right foot first).
- By pressing the R1 + UP arrow button: Walking backward.
- By pressing the R1 + DOWN arrow button: Stop walking.
- By pressing the TRIANGLE button: Vow.
Video Demonstration
Call To Action
If you have any concern regarding this video, please write your message in the comment section.
You might also like to consider supporting my Youtube channel by Subscribing. Click this to Subscribe to TechToTinker.
Thank you and may God bless us all. – George Bantique | tech.to.tinker@gmail.com
Source Code
1/*
2 * Original sourse: https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library
3 * This is the Arduino code PAC6985 16 channel servo controller
4 * watch the video for details and demo http://youtu.be/y8X9X10Tn1k
5 * *
6 Watch V2 Video on using PAC9685 with more code:https://youtu.be/bal2STaoQ1M
7 Watch V3 of this code to control 32 Sevo motor: https://youtu.be/6P21wG7N6t4
8 * Written by Ahmad Shamshiri for Robojax Video channel www.Robojax.com
9 * Date: Dec 16, 2017, in Ajax, Ontario, Canada
10 * Permission granted to share this code given that this
11 * note is kept with the code.
12 * Disclaimer: this code is "AS IS" and for educational purpose only.
13 * this code has been downloaded from http://robojax.com/learn/arduino/
14 *
15 */
16 /***************************************************
17 This is an example for our Adafruit 16-channel PWM & Servo driver
18 Servo test - this will drive 16 servos, one after the other
19 Pick one up today in the adafruit shop!
20 ------> http://www.adafruit.com/products/815
21 These displays use I2C to communicate, 2 pins are required to
22 interface. For Arduino UNOs, thats SCL -> Analog 5, SDA -> Analog 4
23 Adafruit invests time and resources providing this open source code,
24 please support Adafruit and open-source hardware by purchasing
25 products from Adafruit!
26 Written by Limor Fried/Ladyada for Adafruit Industries.
27 BSD license, all text above must be included in any redistribution
28 */
29 /**************************************************
30 * Scheduino_Blink_example
31 *
32 * Simple example using the Scheduino library.
33 * Replicates the behaviour of the standard Arduino 'Blink' example,
34 * while outputting the LED status (ON or OFF) to the Serial port
35 *
36 */
37 #include <Adafruit_PWMServoDriver.h>
38 #include <PS2X_lib.h>
39 #include <SoftwareSerial.h>
40 Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); // create servo Controller Class
41 PS2X ps2x; // create PS2 Controller Class
42 SoftwareSerial mySerial(2,3); // create soft serial class
43 #define SERVOMIN 125 // this is the 'minimum' pulse length count (out of 4096) // RIGHT SIDE
44 #define SERVOMAX 615 // this is the 'maximum' pulse length count (out of 4096) // LEFT SIDE
45 // our servo # counter
46 uint8_t servonum = 0;
47 // Channel 0 - head
48 // Channel 1 - nc
49 // Channel 2 - nc
50 // Channel 3 - left foot
51 // Channel 4 - left knee
52 // Channel 5 - left hip
53 // Channel 6 - left shoulder
54 // Channel 7 - nc
55 // Channel 8 - nc
56 // Channel 9 - nc
57 // Channel 10 - right foot
58 // Channel 11 - right knee
59 // Channel 12 - right hip
60 // Channel 13 - right shoulder
61 // Channel 14 - nc
62 // Channel 15 - nc
63 // Current angle position
64 byte currAnglePOS[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
65 // Maximum allowed angle position
66 byte maxAnglePOS[16] = {0, 0, 0, 115, 180, 180, 180, 0, 0, 0, 115, 180, 180, 180, 0, 0};
67 // Minimum allowed angle position
68 byte minAnglePOS[16] = {0, 0, 0, 65, 0, 0, 0, 0, 0, 0, 65, 0, 0, 0, 0, 0};
69 // Default or standing angle position
70 byte idleAnglePOS[16] = {0, 0, 0, 95, 90, 95, 170, 0, 0, 0, 85, 90, 95, 10, 0, 0};
71 // Requested angle position
72 byte reqAnglePOS[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
73 byte ch = 0; // channel counter
74 int error = 0;
75 byte type = 0;
76 byte vibrate = 0;
77 byte cmdFUNCTION = 0;
78 bool stepRIGHT = 0; // if 1 = forward step right, 0: forward step left
79 bool isWALKING = 0;
80 // ####################################
81 // PROTOTHREADING DEFINITIONS HERE:
82 // ####################################
83 unsigned long checkPS2TaskTimer = 0;
84 const unsigned long checkPS2TaskInterval = 100;
85 unsigned long setCommandTaskTimer = 0;
86 const unsigned long setCommandTaskInterval = 200;
87 unsigned long setServoTaskTimer = 0;
88 const unsigned long setServoTaskInterval = 200;
89 unsigned long doServoTaskTimer = 0;
90 const unsigned long doServoTaskInterval = 20;
91 unsigned long updateLCDTaskTimer = 0;
92 const unsigned long updateLCDTaskInterval = 500;
93 unsigned long updateSerialTaskTimer = 0;
94 const unsigned long updateSerialTaskInterval = 500;
95 // ###############################
96 // PROTOTYPES here:
97 // ###############################
98 unsigned int angleToPulse(byte ang);
99 void doServo ();
100 void setCommand();
101 void setServo();
102 void checkPS2 ();
103 void updateLCD();
104 void updateSerial();
105 // ######################################
106 // setup function:
107 // Put code that run once here
108 // ######################################
109 void setup() {
110 mySerial.begin(9600);
111 Serial.begin(9600);
112 Serial.println("6 DOF Biped Robot");
113 pwm.begin();
114 pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates
115 // copy idle Angle to current Angle
116 for (byte i = 0; i < 16; i++) {
117 reqAnglePOS[i] = idleAnglePOS[i];
118 currAnglePOS[i] = idleAnglePOS[i] - 3;
119 }
120 // Setup PS2 controller Here!
121 //error = ps2x.config_gamepad(12,11,10,13, false, false); //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
122 ps2x.config_gamepad(12,11,10,13, false, false); //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
123 // if(error == 0){
124 // Serial.println("Found Controller, configured successful");
125 // }
126 // else if(error == 1)
127 // Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
128 // else if(error == 2)
129 // Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
130 // else if(error == 3)
131 // Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
132 //
133 // type = ps2x.readType();
134 // switch(type) {
135 // case 0:
136 // Serial.println("Unknown Controller type");
137 // break;
138 // case 1:
139 // Serial.println("DualShock Controller Found");
140 // break;
141 // case 2:
142 // Serial.println("GuitarHero Controller Found");
143 // break;
144 // }
145 mySerial.println(" tech-to-tinker ");
146 Serial.println("Setup DONE");
147 }
148 /* It's best not to do anything in loop() except runTasks() - doing anything else here will affect timing */
149 void loop() {
150 if(millis() >= checkPS2TaskTimer + checkPS2TaskInterval){
151 checkPS2TaskTimer += checkPS2TaskInterval;
152 // do the task
153 checkPS2();
154 Serial.println("checkPS2");
155 }
156 if(millis() >= setCommandTaskTimer + setCommandTaskInterval){
157 setCommandTaskTimer += setCommandTaskInterval;
158 // do the task
159 setCommand();
160 Serial.println("setCommand");
161 }
162 if(millis() >= setServoTaskTimer + setServoTaskInterval){
163 setServoTaskTimer += setServoTaskInterval;
164 // do the task
165 setServo();
166 Serial.println("setServo");
167 }
168 if(millis() >= doServoTaskTimer + doServoTaskInterval){
169 doServoTaskTimer += doServoTaskInterval;
170 // do the task
171 doServo();
172 Serial.println("doServo");
173 }
174 if(millis() >= updateLCDTaskTimer + updateLCDTaskInterval){
175 updateLCDTaskTimer += updateLCDTaskInterval;
176 // do the task
177 updateLCD();
178 Serial.println("updateLCD");
179 }
180 if(millis() >= updateSerialTaskTimer + updateSerialTaskInterval){
181 updateSerialTaskTimer += updateSerialTaskInterval;
182 // do the task
183 updateSerial();
184 Serial.println("updateSerial");
185 }
186 }
187 /*
188 * angleToPulse(int ang)
189 * gets angle in degree and returns the pulse width
190 * also prints the value on seial monitor
191 * written by Ahmad Nejrabi for Robojax, Robojax.com
192 */
193 unsigned int angleToPulse(byte ang){
194 unsigned int pulse = map(ang,0, 180, SERVOMIN,SERVOMAX);// map angle of 0 to 180 to Servo min and Servo max
195 return pulse;
196 }
197 void doServo () {
198 for (ch=0; ch < 16; ch++) {
199 if (reqAnglePOS[ch] < currAnglePOS[ch]) {
200 if ((currAnglePOS[ch] >= reqAnglePOS[ch]) && (currAnglePOS[ch] >= minAnglePOS[ch])) {
201 pwm.setPWM(ch, 0, angleToPulse(currAnglePOS[ch]));
202 currAnglePOS[ch]-=1;
203 }
204 }
205 else if (reqAnglePOS[ch] > currAnglePOS[ch]) {
206 if ((currAnglePOS[ch] <= reqAnglePOS[ch]) && (currAnglePOS[ch] <= maxAnglePOS[ch])) {
207 pwm.setPWM(ch, 0, angleToPulse(currAnglePOS[ch]));
208 currAnglePOS[ch]+=1;
209 }
210 }
211 }
212 }
213 void setCommand() {
214 switch (cmdFUNCTION) {
215 case 0: // no command
216 reqAnglePOS[3] = idleAnglePOS[3];
217 reqAnglePOS[4] = idleAnglePOS[4];
218 reqAnglePOS[5] = idleAnglePOS[5];
219 reqAnglePOS[10] = idleAnglePOS[10];
220 reqAnglePOS[11] = idleAnglePOS[11];
221 reqAnglePOS[12] = idleAnglePOS[12];
222 break;
223 // #############################################################
224 case 1: // pattern 1
225 // Lean to the left
226 reqAnglePOS[3] = idleAnglePOS[3] - 10;
227 reqAnglePOS[4] = idleAnglePOS[4];
228 reqAnglePOS[5] = idleAnglePOS[5];
229 reqAnglePOS[10] = idleAnglePOS[10] - 15;
230 reqAnglePOS[11] = idleAnglePOS[11];
231 reqAnglePOS[12] = idleAnglePOS[12];
232 cmdFUNCTION = 2;
233 break;
234 case 2:
235 cmdFUNCTION = 3;
236 break;
237 case 3:
238 cmdFUNCTION = 4;
239 break;
240 case 4: // pattern 2
241 // Right foot up forward step
242 reqAnglePOS[3] = idleAnglePOS[3] - 10;
243 reqAnglePOS[4] = idleAnglePOS[4] + 20;
244 reqAnglePOS[5] = idleAnglePOS[5] + 20;
245 reqAnglePOS[10] = idleAnglePOS[10] - 15;
246 reqAnglePOS[11] = idleAnglePOS[11] + 20;
247 reqAnglePOS[12] = idleAnglePOS[12] + 20;
248 cmdFUNCTION = 5;
249 break;
250 case 5:
251 cmdFUNCTION = 6;
252 break;
253 case 6:
254 cmdFUNCTION = 7;
255 break;
256 case 7: // pattern 3
257 // Right foot down
258 reqAnglePOS[3] = idleAnglePOS[3] + 15;
259 reqAnglePOS[4] = idleAnglePOS[4] + 20;
260 reqAnglePOS[5] = idleAnglePOS[5] + 20;
261 reqAnglePOS[10] = idleAnglePOS[10] + 10;
262 reqAnglePOS[11] = idleAnglePOS[11] + 20;
263 reqAnglePOS[12] = idleAnglePOS[12] + 20;
264 cmdFUNCTION = 8;
265 break;
266 case 8:
267 cmdFUNCTION = 9;
268 break;
269 case 9:
270 cmdFUNCTION = 10;
271 break;
272 case 10: // pattern 4
273 // Balance foot
274 reqAnglePOS[3] = idleAnglePOS[3] + 25;
275 reqAnglePOS[4] = idleAnglePOS[4] + 20;
276 reqAnglePOS[5] = idleAnglePOS[5] + 20;
277 reqAnglePOS[10] = idleAnglePOS[10] + 10;
278 reqAnglePOS[11] = idleAnglePOS[11] + 0;
279 reqAnglePOS[12] = idleAnglePOS[12] + 0;
280 cmdFUNCTION = 11;
281 break;
282 case 11:
283 cmdFUNCTION = 12;
284 break;
285 case 12:
286 //
287 if (isWALKING) {
288 cmdFUNCTION = 13; // continue walking
289 }
290 else {
291 cmdFUNCTION = 0; // stop walking in upright position
292 }
293 break;
294 // ############################################################
295 case 13: // pattern 5
296 // Lean to the right
297 reqAnglePOS[3] = idleAnglePOS[3] + 15;
298 reqAnglePOS[4] = idleAnglePOS[4];
299 reqAnglePOS[5] = idleAnglePOS[5];
300 reqAnglePOS[10] = idleAnglePOS[10] + 10;
301 reqAnglePOS[11] = idleAnglePOS[11];
302 reqAnglePOS[12] = idleAnglePOS[12];
303 cmdFUNCTION = 14;
304 break;
305 case 14:
306 cmdFUNCTION = 15;
307 break;
308 case 15:
309 cmdFUNCTION = 16;
310 break;
311 case 16: // pattern 6
312 // Left foot up forward step
313 reqAnglePOS[3] = idleAnglePOS[3] + 15;
314 reqAnglePOS[4] = idleAnglePOS[4] - 20;
315 reqAnglePOS[5] = idleAnglePOS[5] - 20;
316 reqAnglePOS[10] = idleAnglePOS[10] + 10;
317 reqAnglePOS[11] = idleAnglePOS[11] - 20;
318 reqAnglePOS[12] = idleAnglePOS[12] - 20;
319 cmdFUNCTION = 17;
320 break;
321 case 17:
322 cmdFUNCTION = 18;
323 break;
324 case 18:
325 cmdFUNCTION = 19;
326 break;
327 case 19: // pattern 7
328 // Left foot down
329 reqAnglePOS[3] = idleAnglePOS[3] - 10;
330 reqAnglePOS[4] = idleAnglePOS[4] - 20;
331 reqAnglePOS[5] = idleAnglePOS[5] - 20;
332 reqAnglePOS[10] = idleAnglePOS[10] - 15;
333 reqAnglePOS[11] = idleAnglePOS[11] - 20;
334 reqAnglePOS[12] = idleAnglePOS[12] - 20;
335 cmdFUNCTION = 20;
336 break;
337 case 20:
338 cmdFUNCTION = 21;
339 break;
340 case 21:
341 cmdFUNCTION = 22;
342 break;
343 case 22: // pattern 8
344 // Balance foot
345 reqAnglePOS[3] = idleAnglePOS[3] - 10;
346 reqAnglePOS[4] = idleAnglePOS[4] - 0;
347 reqAnglePOS[5] = idleAnglePOS[5] - 0;
348 reqAnglePOS[10] = idleAnglePOS[10] - 25;
349 reqAnglePOS[11] = idleAnglePOS[11] - 20;
350 reqAnglePOS[12] = idleAnglePOS[12] - 20;
351 cmdFUNCTION = 23;
352 break;
353 case 23:
354 cmdFUNCTION = 24;
355 break;
356 case 24:
357 if (isWALKING) {
358 cmdFUNCTION = 1; // continue walking
359 }
360 else {
361 cmdFUNCTION = 0; // stop walking in upright position
362 }
363 break;
364 // #########################################################
365 case 25: // Vow
366 reqAnglePOS[3] = idleAnglePOS[3];
367 reqAnglePOS[4] = idleAnglePOS[4];
368 reqAnglePOS[5] = idleAnglePOS[5];
369 reqAnglePOS[10] = idleAnglePOS[10];
370 reqAnglePOS[11] = idleAnglePOS[11];
371 reqAnglePOS[12] = idleAnglePOS[12];
372 cmdFUNCTION = 26;
373 break;
374 case 26:
375 cmdFUNCTION = 27;
376 break;
377 case 27:
378 cmdFUNCTION = 28;
379 break;
380 case 28:
381 reqAnglePOS[3] = idleAnglePOS[3];
382 reqAnglePOS[4] = idleAnglePOS[4];
383 reqAnglePOS[5] = idleAnglePOS[5] - 90;
384 reqAnglePOS[10] = idleAnglePOS[10];
385 reqAnglePOS[11] = idleAnglePOS[11];
386 reqAnglePOS[12] = idleAnglePOS[12] + 90;
387 cmdFUNCTION = 29;
388 break;
389 case 29:
390 cmdFUNCTION = 30;
391 break;
392 case 30:
393 cmdFUNCTION = 0; // stand straight
394 break;
395 // ########################################################
396 case 31: // pattern 4 REVERSE
397 // Balance foot
398 reqAnglePOS[3] = idleAnglePOS[3] + 25;
399 reqAnglePOS[4] = idleAnglePOS[4] + 20;
400 reqAnglePOS[5] = idleAnglePOS[5] + 20;
401 reqAnglePOS[10] = idleAnglePOS[10] + 10;
402 reqAnglePOS[11] = idleAnglePOS[11] + 0;
403 reqAnglePOS[12] = idleAnglePOS[12] + 0;
404 cmdFUNCTION = 32;
405 break;
406 case 32:
407 cmdFUNCTION = 33;
408 break;
409 case 33:
410 cmdFUNCTION = 34;
411 break;
412 case 34: // pattern 3 REVERSE
413 // Right foot down
414 reqAnglePOS[3] = idleAnglePOS[3] + 15;
415 reqAnglePOS[4] = idleAnglePOS[4] + 20;
416 reqAnglePOS[5] = idleAnglePOS[5] + 20;
417 reqAnglePOS[10] = idleAnglePOS[10] + 10;
418 reqAnglePOS[11] = idleAnglePOS[11] + 20;
419 reqAnglePOS[12] = idleAnglePOS[12] + 20;
420 cmdFUNCTION = 35;
421 break;
422 case 35:
423 cmdFUNCTION = 36;
424 break;
425 case 36:
426 cmdFUNCTION = 37;
427 break;
428 case 37: // pattern 2 REVERSE
429 // Right foot up forward step
430 reqAnglePOS[3] = idleAnglePOS[3] - 10;
431 reqAnglePOS[4] = idleAnglePOS[4] + 20;
432 reqAnglePOS[5] = idleAnglePOS[5] + 20;
433 reqAnglePOS[10] = idleAnglePOS[10] - 15;
434 reqAnglePOS[11] = idleAnglePOS[11] + 20;
435 reqAnglePOS[12] = idleAnglePOS[12] + 20;
436 cmdFUNCTION = 38;
437 break;
438 case 38:
439 cmdFUNCTION = 39;
440 break;
441 case 39:
442 cmdFUNCTION = 40;
443 break;
444 case 40: // pattern 1 REVERSE
445 // Lean to the left
446 reqAnglePOS[3] = idleAnglePOS[3] - 10;
447 reqAnglePOS[4] = idleAnglePOS[4];
448 reqAnglePOS[5] = idleAnglePOS[5];
449 reqAnglePOS[10] = idleAnglePOS[10] - 15;
450 reqAnglePOS[11] = idleAnglePOS[11];
451 reqAnglePOS[12] = idleAnglePOS[12];
452 cmdFUNCTION = 41;
453 break;
454 case 41:
455 cmdFUNCTION = 42;
456 break;
457 case 42:
458 if (isWALKING) {
459 cmdFUNCTION = 43; // continue walking
460 }
461 else {
462 cmdFUNCTION = 0; // stop walking in upright position
463 }
464 break;
465 // ###########################################
466 case 43: // pattern 8 REVERSE
467 // Balance foot
468 reqAnglePOS[3] = idleAnglePOS[3] - 10;
469 reqAnglePOS[4] = idleAnglePOS[4] - 0;
470 reqAnglePOS[5] = idleAnglePOS[5] - 0;
471 reqAnglePOS[10] = idleAnglePOS[10] - 25;
472 reqAnglePOS[11] = idleAnglePOS[11] - 20;
473 reqAnglePOS[12] = idleAnglePOS[12] - 20;
474 cmdFUNCTION = 44;
475 break;
476 case 44:
477 cmdFUNCTION = 45;
478 break;
479 case 45:
480 cmdFUNCTION = 46;
481 break;
482 case 46: // pattern 7 REVERSE
483 // Left foot down
484 reqAnglePOS[3] = idleAnglePOS[3] - 10;
485 reqAnglePOS[4] = idleAnglePOS[4] - 20;
486 reqAnglePOS[5] = idleAnglePOS[5] - 20;
487 reqAnglePOS[10] = idleAnglePOS[10] - 15;
488 reqAnglePOS[11] = idleAnglePOS[11] - 20;
489 reqAnglePOS[12] = idleAnglePOS[12] - 20;
490 cmdFUNCTION = 47;
491 break;
492 case 47:
493 cmdFUNCTION = 48;
494 break;
495 case 48:
496 cmdFUNCTION = 49;
497 break;
498 case 49: // pattern 6 REVERSE
499 // Left foot up forward step
500 reqAnglePOS[3] = idleAnglePOS[3] + 15;
501 reqAnglePOS[4] = idleAnglePOS[4] - 20;
502 reqAnglePOS[5] = idleAnglePOS[5] - 20;
503 reqAnglePOS[10] = idleAnglePOS[10] + 10;
504 reqAnglePOS[11] = idleAnglePOS[11] - 20;
505 reqAnglePOS[12] = idleAnglePOS[12] - 20;
506 cmdFUNCTION = 50;
507 break;
508 case 50:
509 cmdFUNCTION = 51;
510 break;
511 case 51:
512 cmdFUNCTION = 52;
513 break;
514 case 52: // pattern 5 REVERSE
515 // Lean to the right
516 reqAnglePOS[3] = idleAnglePOS[3] + 15;
517 reqAnglePOS[4] = idleAnglePOS[4];
518 reqAnglePOS[5] = idleAnglePOS[5];
519 reqAnglePOS[10] = idleAnglePOS[10] + 10;
520 reqAnglePOS[11] = idleAnglePOS[11];
521 reqAnglePOS[12] = idleAnglePOS[12];
522 if (isWALKING) {
523 cmdFUNCTION = 31; // continue walking
524 }
525 else {
526 cmdFUNCTION = 0; // stop walking in upright position
527 }
528 break;
529 default:
530 break;
531 }
532 }
533 void setServo() {
534 // ##################
535 // ### COMMANDS ###
536 // ##################
537 if(ps2x.Button(PSB_R1)) {
538 // Reverse function
539 if (ps2x.ButtonPressed(PSB_PAD_UP)) {
540 isWALKING = 1;
541 cmdFUNCTION = 31;
542 }
543 if (ps2x.ButtonReleased(PSB_PAD_UP)) {
544 isWALKING = 0;
545 cmdFUNCTION = 0;
546 }
547 if (ps2x.Button(PSB_PAD_LEFT)) {
548 isWALKING = 0;
549 cmdFUNCTION = 31; // left foot backward
550 }
551 if (ps2x.Button(PSB_PAD_RIGHT)) {
552 isWALKING = 0;
553 cmdFUNCTION = 43; // right foot back ward
554 }
555 if (ps2x.Button(PSB_PAD_DOWN)) {
556 cmdFUNCTION = 0; //
557 }
558 if(ps2x.Button(PSB_GREEN)) {
559 cmdFUNCTION = 25;
560 }
561 }
562 else {
563 // Forward function
564 if (ps2x.ButtonPressed(PSB_PAD_UP)) {
565 isWALKING = 1;
566 cmdFUNCTION = 1;
567 }
568 if (ps2x.ButtonReleased(PSB_PAD_UP)) {
569 isWALKING = 0;
570 cmdFUNCTION = 0;
571 }
572 if (ps2x.Button(PSB_PAD_LEFT)) {
573 isWALKING = 0;
574 cmdFUNCTION = 13; // left foot forward
575 }
576 if (ps2x.Button(PSB_PAD_RIGHT)) {
577 isWALKING = 0;
578 cmdFUNCTION = 1; // right foot forward
579 }
580 if (ps2x.Button(PSB_PAD_DOWN)) {
581 cmdFUNCTION = 0; //
582 }
583 if(ps2x.Button(PSB_GREEN)) {
584 cmdFUNCTION = 25;
585 }
586 }
587 // // ##################
588 // // ### LEFT SIDE ###
589 // // ##################
590 // if (ps2x.Button(PSB_L3)) {
591 // reqAnglePOS[3] = map(ps2x.Analog(PSS_LX), 0, 255, 0, 180);
592 // }
593 //
594 // if(ps2x.Button(PSB_L1)) {
595 // reqAnglePOS[4] = map(ps2x.Analog(PSS_LY), 255, 0, 0, 180);
596 // }
597 //
598 // if(ps2x.Button(PSB_L2)) {
599 // reqAnglePOS[5] = map(ps2x.Analog(PSS_LY), 0, 255, 0, 180);
600 // }
601 //
602 //
603 // // ##################
604 // // ### RIGHT SIDE ###
605 // // ##################
606 // if (ps2x.Button(PSB_R3)) {
607 // reqAnglePOS[10] = map(ps2x.Analog(PSS_RX), 0, 255, 0, 180);
608 // }
609 //
610 // if(ps2x.Button(PSB_R1)) {
611 // reqAnglePOS[11] = map(ps2x.Analog(PSS_RY), 0, 255, 0, 180);
612 // }
613 //
614 // if(ps2x.Button(PSB_R2)) {
615 // reqAnglePOS[12] = map(ps2x.Analog(PSS_RY), 255, 0, 0, 180);
616 // }
617 // #######################
618 // ### RESET / DEFAULT ###
619 // #######################
620 // Reset to default when START button is pressed!
621 if (ps2x.Button(PSB_START)) {
622 cmdFUNCTION = 0;
623 }
624 }
625 void checkPS2 () {
626 ps2x.read_gamepad();
627 }
628 void updateLCD() {
629 mySerial.println(" from 6-DOF Robot ");
630 }
631 void updateSerial() {
632 Serial.print(reqAnglePOS[3]);
633 Serial.print(", ");
634 Serial.print(reqAnglePOS[4]);
635 Serial.print(", ");
636 Serial.print(reqAnglePOS[5]);
637 Serial.print(", ");
638 Serial.print(reqAnglePOS[10]);
639 Serial.print(", ");
640 Serial.print(reqAnglePOS[11]);
641 Serial.print(", ");
642 Serial.print(reqAnglePOS[12]);
643 Serial.println("");
644 }
Posts in this series
- How to Get Started with ATTiny85 in Arduino IDE
- Tutorial: How to use MFRC522 RFID module using Arduino
- SOS Flasher Using Millis Function with Enable Switch
- Tutorial: How to use DS3231 RTC in Arduino
- Tutorial: How to use 0.96 OLED - a small and cute display
- Tutorial: Getting Started with the NRF24L01 | How to use | Arduino
- Tutorial: How to use SIM800L GSM Module for Controlling Anything | Arduino
- Tutorial: How to use Keypad | Text Entry Mode | Arduino
- Tutorial: How to use 4x4 Keypad | Arduino
- Project Idea: Arduino Voltmeter
- Project Idea: Door Lock Security | Arduino
- Multitasking with Arduino | Relay Timer Controller | using millis
- Tutorial Understanding Blink Without Delay | How to millis
- Arduino Simple LCD Menu
- How to use SIM800L GSM Module using Arduino | Make or Answer Voice Calls
- Tutorial: How to Use Arduino Uno as HID | Part 2: Arduino Mouse Emulation
- Tutorial: How to Use Arduino Uno as HID | Part 1: Arduino Keyboard Emulation
- Tutorial: How to use SIM800L DTMF to Control Anything | Arduino
- Tutorial: Arduino EEPROM
- How to use SIM800L GSM Module | Arduino | Send and Receive SMS
- 16x2 LCD Menu for Arduino
- Tutorial: Arduino GPIO | How to use Arduino Pins
- MIT App Inventor for Arduino
- RC Car using L298N, HC-06, and Arduino Uno
- How to Use LCD Keypad Shield for Arduino
- How to Use Arduino Interrupts
- Project: Automatic Alcohol Dispenser
- TUTORIAL: How to use HC-SR04 Ultrasonic Sensor with Arduino
- Source Code: Astronomia Meme and Funeral Dance | melodies the Arduino way
- How to Get Started with L293D Motor Driver Shield with Arduino
- How to Get Started with L298N Motor Driver module using Arduino
- Part 2: Wav Music Player with Lyrics Using Arduino and SD Card
- Interfacing Infrared to Arduino Uno
- Part 1: Wav Music Player Using Arduino Uno and SD Card
- How to Interface Stepper Motor to Arduino Uno
- How To Play MP3 Files on Arduino from SD Card
- What is Arduino Software Serial
- How to Interface SD card to Arduino (without SD card shield)?
- Playing Melodies Using Arduino
- How to Interface PS2 Controller to Arduino Uno
- Part 3: DF Player Mini Tinkering with Arduino Nano and LCD
- How to Interface HC-06 to Arduino
- How to make a Remote Control RC car using Arduino and HC-06 bluetooth module
- Part 2: DF Player Mini Tinkering with Arduino Nano
- Part 1: DF Player Mini - a mini cheap mp3 player
No comments yet!