├── .DS_Store ├── README.md ├── block-diagrams ├── .DS_Store ├── flex-sensor-and-mpu6050_bb.jpg ├── flex-sensor-glove_bb.png ├── flex-sensor-test_bb.png ├── nema-17-with-driver_bb.png ├── servo-motor-robotic-arm-only_bb.png ├── servo-motor-robotic-arm.fzz ├── servo-motor-robotic-arm_bb.png └── servo-motor-single-motor.fzz └── code ├── .DS_Store ├── arduino_robot_arm_code.zip ├── arduino_robot_arm_code └── arduino_robot_arm_code.ino ├── robotic_glove_code.zip ├── robotic_glove_code └── robotic_glove_code.ino └── test-code ├── .DS_Store ├── flex_sensor_test └── flex_sensor_test.ino ├── mpu6050-degrees-test └── mpu6050-degrees-test.ino ├── nema_17_motor_test └── nema_17_motor_test.ino └── servo_motor_single └── servo_motor_single.ino /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EbenKouao/arduino-robot-arm/57041c18da89bd6983156740ac52b19ef2cfc468/.DS_Store -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Arduino Robotic Arm - Controlled by Hand Gestures 2 | 3 | This video is a tutorial on how to build a DIY Robotic Arm controlled by hand gestures. The Robotic arm is able to be controlled wirelessly from a distance as well as perform basic functions on its own. 4 | 5 | Find the Full Instruction Tutorial on smartbuilds.io 6 | For more tutorials subscribe to my [YouTube channel](https://www.youtube.com/channel/UCGxwyXJWEarxh2XWqvygiIg) 7 | 8 | Robot Arm: Thingiverse 3D Printed Robotic Arm - [credits: Wonder Tiger](https://www.thingiverse.com/thing:1748596) 9 | 10 | - Part 1: Gripper Parts 11 | - Part 2: Base and Turntable 12 | - Part 3: Robotic Core Arm 13 | 14 | Robotic Glove: Thingyverse Robotic Glove/ Gauntlet - [(credits: Roman 13)](https://www.thingiverse.com/thing:1982745) 15 | 16 | - Robotic Glove 17 | -------------------------------------------------------------------------------- /block-diagrams/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EbenKouao/arduino-robot-arm/57041c18da89bd6983156740ac52b19ef2cfc468/block-diagrams/.DS_Store -------------------------------------------------------------------------------- /block-diagrams/flex-sensor-and-mpu6050_bb.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EbenKouao/arduino-robot-arm/57041c18da89bd6983156740ac52b19ef2cfc468/block-diagrams/flex-sensor-and-mpu6050_bb.jpg -------------------------------------------------------------------------------- /block-diagrams/flex-sensor-glove_bb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EbenKouao/arduino-robot-arm/57041c18da89bd6983156740ac52b19ef2cfc468/block-diagrams/flex-sensor-glove_bb.png -------------------------------------------------------------------------------- /block-diagrams/flex-sensor-test_bb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EbenKouao/arduino-robot-arm/57041c18da89bd6983156740ac52b19ef2cfc468/block-diagrams/flex-sensor-test_bb.png -------------------------------------------------------------------------------- /block-diagrams/nema-17-with-driver_bb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EbenKouao/arduino-robot-arm/57041c18da89bd6983156740ac52b19ef2cfc468/block-diagrams/nema-17-with-driver_bb.png -------------------------------------------------------------------------------- /block-diagrams/servo-motor-robotic-arm-only_bb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EbenKouao/arduino-robot-arm/57041c18da89bd6983156740ac52b19ef2cfc468/block-diagrams/servo-motor-robotic-arm-only_bb.png -------------------------------------------------------------------------------- /block-diagrams/servo-motor-robotic-arm.fzz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EbenKouao/arduino-robot-arm/57041c18da89bd6983156740ac52b19ef2cfc468/block-diagrams/servo-motor-robotic-arm.fzz -------------------------------------------------------------------------------- /block-diagrams/servo-motor-robotic-arm_bb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EbenKouao/arduino-robot-arm/57041c18da89bd6983156740ac52b19ef2cfc468/block-diagrams/servo-motor-robotic-arm_bb.png -------------------------------------------------------------------------------- /block-diagrams/servo-motor-single-motor.fzz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EbenKouao/arduino-robot-arm/57041c18da89bd6983156740ac52b19ef2cfc468/block-diagrams/servo-motor-single-motor.fzz -------------------------------------------------------------------------------- /code/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EbenKouao/arduino-robot-arm/57041c18da89bd6983156740ac52b19ef2cfc468/code/.DS_Store -------------------------------------------------------------------------------- /code/arduino_robot_arm_code.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EbenKouao/arduino-robot-arm/57041c18da89bd6983156740ac52b19ef2cfc468/code/arduino_robot_arm_code.zip -------------------------------------------------------------------------------- /code/arduino_robot_arm_code/arduino_robot_arm_code.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Author: Smartbuilds.io 3 | YouTube: https://www.youtube.com/channel/UCGxwyXJWEarxh2XWqvygiIg 4 | Fork your own version: https://github.com/EbenKouao/arduino-robot-arm 5 | Check out the full article: https://smartbuilds.io/diy-arduino-robot-arm-controlled-hand-gestures/ 6 | Date: 06/01/2021 7 | Robot Arm 8 | Version 1.0 9 | Creator: smartbuilds.io 10 | Description: Robotic Arm Mark II - Servo Motor 11 | 12 | To use the module connect it to your Arduino as follows: 13 | 14 | PCA9685...........Uno/Nano 15 | GND...............GND 16 | OE................N/A 17 | SCL...............A5 18 | SDA...............A4 19 | VCC...............5V 20 | 21 | ******************************************************************************/ 22 | 23 | /* Include the HCPCA9685 library */ 24 | #include "HCPCA9685.h" 25 | 26 | /* I2C slave address for the device/module. For the HCMODU0097 the default I2C address 27 | is 0x40 */ 28 | #define I2CAdd 0x40 29 | 30 | /* Create an instance of the library */ 31 | HCPCA9685 HCPCA9685(I2CAdd); 32 | 33 | //initial parking position of the motor 34 | const int servo_joint_L_parking_pos = 60; 35 | const int servo_joint_R_parking_pos = 60; 36 | const int servo_joint_1_parking_pos = 70; 37 | const int servo_joint_2_parking_pos = 47; 38 | const int servo_joint_3_parking_pos = 63; 39 | const int servo_joint_4_parking_pos = 63; 40 | 41 | //Degree of robot servo sensitivity - Intervals 42 | int servo_joint_L_pos_increment = 20; 43 | int servo_joint_R_pos_increment = 20; 44 | int servo_joint_1_pos_increment = 20; 45 | int servo_joint_2_pos_increment = 50; 46 | int servo_joint_3_pos_increment = 60; 47 | int servo_joint_4_pos_increment = 40; 48 | 49 | //Keep track of the current value of the motor positions 50 | int servo_joint_L_parking_pos_i = servo_joint_L_parking_pos; 51 | int servo_joint_R_parking_pos_i = servo_joint_R_parking_pos; 52 | int servo_joint_1_parking_pos_i = servo_joint_1_parking_pos; 53 | int servo_joint_2_parking_pos_i = servo_joint_2_parking_pos; 54 | int servo_joint_3_parking_pos_i = servo_joint_3_parking_pos; 55 | int servo_joint_4_parking_pos_i = servo_joint_4_parking_pos; 56 | 57 | 58 | //Minimum and maximum angle of servo motor 59 | int servo_joint_L_min_pos = 10; 60 | int servo_joint_L_max_pos = 180; 61 | 62 | int servo_joint_R_min_pos = 10; 63 | int servo_joint_R_max_pos = 180; 64 | 65 | int servo_joint_1_min_pos = 10; 66 | int servo_joint_1_max_pos = 400; 67 | 68 | int servo_joint_2_min_pos = 10; 69 | int servo_joint_2_max_pos = 380; 70 | 71 | int servo_joint_3_min_pos = 10; 72 | int servo_joint_3_max_pos = 380; 73 | 74 | int servo_joint_4_min_pos = 10; 75 | int servo_joint_4_max_pos = 120; 76 | 77 | int servo_L_pos = 0; 78 | int servo_R_pos = 0; 79 | int servo_joint_1_pos = 0; 80 | int servo_joint_2_pos = 0; 81 | int servo_joint_3_pos = 0; 82 | int servo_joint_4_pos = 0; 83 | 84 | char state = 0; // Changes value from ASCII to char 85 | int response_time = 5; 86 | int response_time_4 = 2; 87 | int loop_check = 0; 88 | int response_time_fast = 20; 89 | int action_delay = 600; 90 | 91 | //Posiion of motor for example demos 92 | unsigned int Pos; 93 | 94 | // Define pin connections & motor's steps per revolution 95 | const int dirPin = 4; 96 | const int stepPin = 5; 97 | const int stepsPerRevolution = 120; 98 | int stepDelay = 4500; 99 | const int stepsPerRevolutionSmall = 60; 100 | int stepDelaySmall = 9500; 101 | 102 | void setup() 103 | { 104 | // Declare pins as Outputs 105 | pinMode(stepPin, OUTPUT); 106 | pinMode(dirPin, OUTPUT); 107 | 108 | /* Initialise the library and set it to 'servo mode' */ 109 | HCPCA9685.Init(SERVO_MODE); 110 | 111 | /* Wake the device up */ 112 | HCPCA9685.Sleep(false); 113 | 114 | Serial.begin(4800); // Initialise default communication rate of the Bluetooth module 115 | 116 | 117 | delay(3000); 118 | //wakeUp(); -- Uncomment for Example Demo 1 119 | //flexMotors(); -- Uncomment for Example Demo 1 120 | 121 | } 122 | 123 | 124 | void loop() { 125 | 126 | 127 | if (Serial.available() > 0) { // Checks whether data is coming from the serial port 128 | 129 | state = Serial.read(); // Reads the data from the serial port 130 | Serial.print(state); // Prints out the value sent 131 | 132 | 133 | //For the naming of the motors, refer to the article / tutorial 134 | //Move (Base Rotation) Stepper Motor Left 135 | if (state == 'S') { 136 | baseRotateLeft(); 137 | delay(response_time); 138 | } 139 | 140 | //Move (Base Rotation) Stepper Motor Right 141 | if (state == 'O') { 142 | baseRotateRight(); 143 | delay(response_time); 144 | } 145 | 146 | 147 | //Move Shoulder Down 148 | if (state == 'c') { 149 | 150 | shoulderServoForward(); 151 | delay(response_time); 152 | 153 | } 154 | 155 | //Move Shoulder Up 156 | if (state == 'C') { 157 | 158 | shoulderServoBackward(); 159 | delay(response_time); 160 | 161 | } 162 | 163 | //Move Elbow Down 164 | if (state == 'p') { 165 | 166 | elbowServoForward(); 167 | delay(response_time); 168 | 169 | } 170 | 171 | //Move Elbow Up 172 | if (state == 'P') { 173 | 174 | elbowServoBackward(); 175 | delay(response_time); 176 | 177 | } 178 | 179 | 180 | //Move Wrist 1 UP 181 | if (state == 'U') { 182 | 183 | wristServo1Backward(); 184 | delay(response_time); 185 | } 186 | 187 | //Move Move Wrist 1 Down 188 | if (state == 'G') { 189 | 190 | wristServo1Forward(); 191 | delay(response_time); 192 | 193 | } 194 | 195 | 196 | //Move Wrist 2 Clockwise 197 | if (state == 'R') { 198 | 199 | wristServoCW(); 200 | delay(response_time); 201 | 202 | } 203 | 204 | //Move Wrist 2 Counter-CW 205 | if (state == 'L') { 206 | 207 | wristServoCCW(); 208 | delay(response_time); 209 | 210 | } 211 | 212 | 213 | //Open Claw Grip 214 | if (state == 'F') { 215 | gripperServoBackward(); 216 | delay(response_time); 217 | 218 | } 219 | 220 | //Close Claw Grip 221 | if (state == 'f') { 222 | gripperServoForward(); 223 | delay(response_time); 224 | } 225 | 226 | 227 | } 228 | } 229 | 230 | //Boiler plate function - These functions move the servo motors in a specific direction for a duration. 231 | 232 | void gripperServoForward() { 233 | 234 | if (servo_joint_4_parking_pos_i > servo_joint_4_min_pos) { 235 | HCPCA9685.Servo(5, servo_joint_4_parking_pos_i); 236 | delay(response_time); //Delay the time takee to turn the servo by the given increment 237 | Serial.println(servo_joint_4_parking_pos_i); 238 | servo_joint_4_parking_pos_i = servo_joint_4_parking_pos_i - servo_joint_4_pos_increment; 239 | 240 | } 241 | } 242 | 243 | void gripperServoBackward() { 244 | 245 | if (servo_joint_4_parking_pos_i < servo_joint_4_max_pos) { 246 | HCPCA9685.Servo(5, servo_joint_4_parking_pos_i); 247 | delay(response_time); 248 | Serial.println(servo_joint_4_parking_pos_i); 249 | servo_joint_4_parking_pos_i = servo_joint_4_parking_pos_i + servo_joint_4_pos_increment; 250 | 251 | } 252 | 253 | } 254 | 255 | void wristServoCW() { 256 | 257 | if (servo_joint_3_parking_pos_i > servo_joint_3_min_pos) { 258 | HCPCA9685.Servo(4, servo_joint_3_parking_pos_i); 259 | delay(response_time_4); 260 | Serial.println(servo_joint_3_parking_pos_i); 261 | servo_joint_3_parking_pos_i = servo_joint_3_parking_pos_i - servo_joint_3_pos_increment; 262 | 263 | } 264 | 265 | } 266 | 267 | void wristServoCCW() { 268 | 269 | if (servo_joint_3_parking_pos_i < servo_joint_3_max_pos) { 270 | HCPCA9685.Servo(4, servo_joint_3_parking_pos_i); 271 | delay(response_time_4); 272 | Serial.println(servo_joint_3_parking_pos_i); 273 | servo_joint_3_parking_pos_i = servo_joint_3_parking_pos_i + servo_joint_3_pos_increment; 274 | 275 | } 276 | 277 | } 278 | 279 | void wristServo1Forward() { 280 | 281 | if (servo_joint_2_parking_pos_i < servo_joint_2_max_pos) { 282 | HCPCA9685.Servo(3, servo_joint_2_parking_pos_i); 283 | delay(response_time); 284 | Serial.println(servo_joint_2_parking_pos_i); 285 | 286 | servo_joint_2_parking_pos_i = servo_joint_2_parking_pos_i + servo_joint_2_pos_increment; 287 | 288 | } 289 | 290 | 291 | } 292 | 293 | void wristServo1Backward() { 294 | 295 | if (servo_joint_2_parking_pos_i > servo_joint_2_min_pos) { 296 | HCPCA9685.Servo(3, servo_joint_2_parking_pos_i); 297 | delay(response_time); 298 | Serial.println(servo_joint_2_parking_pos_i); 299 | 300 | servo_joint_2_parking_pos_i = servo_joint_2_parking_pos_i - servo_joint_2_pos_increment; 301 | 302 | } 303 | 304 | } 305 | 306 | 307 | void elbowServoForward() { 308 | 309 | if (servo_joint_L_parking_pos_i < servo_joint_L_max_pos) { 310 | HCPCA9685.Servo(0, servo_joint_L_parking_pos_i); 311 | HCPCA9685.Servo(1, (servo_joint_L_max_pos - servo_joint_L_parking_pos_i)); 312 | 313 | delay(response_time); 314 | Serial.println(servo_joint_L_parking_pos_i); 315 | 316 | servo_joint_L_parking_pos_i = servo_joint_L_parking_pos_i + servo_joint_L_pos_increment; 317 | servo_joint_R_parking_pos_i = servo_joint_L_max_pos - servo_joint_L_parking_pos_i; 318 | 319 | } 320 | } 321 | 322 | void elbowServoBackward() { 323 | if (servo_joint_L_parking_pos_i > servo_joint_L_min_pos) { 324 | HCPCA9685.Servo(0, servo_joint_L_parking_pos_i); 325 | HCPCA9685.Servo(1, (servo_joint_L_max_pos - servo_joint_L_parking_pos_i)); 326 | 327 | delay(response_time); 328 | Serial.println(servo_joint_L_parking_pos_i); 329 | 330 | 331 | servo_joint_L_parking_pos_i = servo_joint_L_parking_pos_i - servo_joint_L_pos_increment; 332 | servo_joint_R_parking_pos_i = servo_joint_L_max_pos - servo_joint_L_parking_pos_i; 333 | 334 | } 335 | 336 | } 337 | 338 | void shoulderServoForward() { 339 | 340 | if (servo_joint_1_parking_pos_i < servo_joint_1_max_pos) { 341 | HCPCA9685.Servo(2, servo_joint_1_parking_pos_i); 342 | delay(response_time); 343 | Serial.println(servo_joint_1_parking_pos_i); 344 | 345 | servo_joint_1_parking_pos_i = servo_joint_1_parking_pos_i + servo_joint_1_pos_increment; 346 | 347 | } 348 | 349 | } 350 | 351 | void shoulderServoBackward() { 352 | 353 | 354 | if (servo_joint_1_parking_pos_i > servo_joint_1_min_pos) { 355 | HCPCA9685.Servo(2, servo_joint_1_parking_pos_i); 356 | delay(response_time); 357 | Serial.println(servo_joint_1_parking_pos_i); 358 | 359 | servo_joint_1_parking_pos_i = servo_joint_1_parking_pos_i - servo_joint_1_pos_increment; 360 | 361 | } 362 | } 363 | 364 | void baseRotateLeft() { 365 | //clockwise 366 | digitalWrite(dirPin, HIGH); 367 | // Spin motor 368 | for (int x = 0; x < stepsPerRevolution; x++) 369 | { 370 | digitalWrite(stepPin, HIGH); 371 | delayMicroseconds(stepDelay); 372 | digitalWrite(stepPin, LOW); 373 | delayMicroseconds(stepDelay); 374 | } 375 | delay(response_time); // Wait a second 376 | } 377 | 378 | 379 | void baseRotateRight() { 380 | 381 | //counterclockwise 382 | digitalWrite(dirPin, LOW); 383 | // Spin motor 384 | for (int x = 0; x < stepsPerRevolution; x++) 385 | { 386 | digitalWrite(stepPin, HIGH); 387 | delayMicroseconds(stepDelay); 388 | digitalWrite(stepPin, LOW); 389 | delayMicroseconds(stepDelay); 390 | } 391 | delay(response_time); // Wait a second 392 | } 393 | 394 | void wakeUp() { 395 | 396 | //Pre-Program Function - Wake Up Robot on Start 397 | 398 | if (loop_check == 0) { 399 | 400 | // //Shoulder Raise 401 | for (Pos = 0; Pos < 10; Pos++) 402 | { 403 | 404 | HCPCA9685.Servo(1, Pos); 405 | delay(response_time_fast); 406 | } 407 | 408 | // //Move Elbow Backwards 409 | for (Pos = 400; Pos > 390; Pos--) 410 | { 411 | 412 | HCPCA9685.Servo(2, Pos); 413 | 414 | delay(response_time_fast); 415 | } 416 | 417 | //Move Wrist 1 Forward 418 | for (Pos = 10; Pos < 20; Pos++) 419 | { 420 | HCPCA9685.Servo(3, Pos); 421 | delay(response_time); 422 | } 423 | 424 | //Move Wrist 2 Backwards 425 | for (Pos = 380; Pos > 50; Pos--) 426 | { 427 | HCPCA9685.Servo(4, Pos); 428 | delay(response_time); 429 | } 430 | 431 | //Move Wrist 2 Backwards 432 | for (Pos = 50; Pos < 150; Pos++) 433 | { 434 | HCPCA9685.Servo(4, Pos); 435 | delay(response_time); 436 | } 437 | 438 | //Move Wrist 1 Forward 439 | for (Pos = 19; Pos < 100; Pos++) 440 | { 441 | HCPCA9685.Servo(3, Pos); 442 | delay(response_time); 443 | } 444 | loop_check = 0; 445 | 446 | } 447 | } 448 | 449 | void flexMotors() { 450 | 451 | //Example Demo Pre-program Function to Make Robot Wake Up (Motor by Motor) 452 | 453 | if (loop_check == 0) { 454 | 455 | delay(action_delay); 456 | 457 | //Move Wrist 1 Forward 458 | for (Pos = 100; Pos > 10; Pos--) 459 | { 460 | HCPCA9685.Servo(3, Pos); 461 | delay(10); 462 | } 463 | 464 | delay(action_delay); 465 | 466 | //Move Wrist 1 Forward 467 | for (Pos = 10; Pos < 70; Pos++) 468 | { 469 | HCPCA9685.Servo(3, Pos); 470 | delay(10); 471 | } 472 | 473 | delay(action_delay); 474 | 475 | baseRotateLeft(); 476 | delay(action_delay); 477 | 478 | 479 | //Move Wrist 2 Backwards 480 | for (Pos = 200; Pos < 380; Pos++) 481 | { 482 | HCPCA9685.Servo(4, Pos); 483 | delay(10); 484 | } 485 | 486 | delay(action_delay); 487 | 488 | loop_check = 1; 489 | 490 | 491 | } 492 | } 493 | -------------------------------------------------------------------------------- /code/robotic_glove_code.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EbenKouao/arduino-robot-arm/57041c18da89bd6983156740ac52b19ef2cfc468/code/robotic_glove_code.zip -------------------------------------------------------------------------------- /code/robotic_glove_code/robotic_glove_code.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Author: Smartbuilds.io 3 | YouTube: https://www.youtube.com/channel/UCGxwyXJWEarxh2XWqvygiIg 4 | Fork your own version: https://github.com/EbenKouao/arduino-robotic-arm 5 | Date: 28/12/2020 6 | Description: Robotic Glove to control the Robotic Arm using Bluetooth 7 | 8 | ******************************************************************************/ 9 | 10 | #include 11 | 12 | //Create thumb Sensors 13 | int pinkie = 0; //Pinkie thumb 14 | int finger = 0; //finger thumb 15 | int thumb = 0; //Index thumb 16 | 17 | int pinkie_Data = A1; 18 | int finger_Data = A2; 19 | int thumb_Data = A3; 20 | 21 | //const int MPU_addr = 0x68; 22 | const int MPU2 = 0x69, MPU1 = 0x68; 23 | 24 | //First MPU6050 25 | int16_t AcX1, AcY1, AcZ1, Tmp1, GyX1, GyY1, GyZ1; 26 | int minVal = 265; 27 | int maxVal = 402; 28 | double x; 29 | double y; 30 | double z; 31 | 32 | //Second MPU6050 33 | int16_t AcX2, AcY2, AcZ2, Tmp2, GyX2, GyY2, GyZ2; 34 | int minVal2 = 265; 35 | int maxVal2 = 402; 36 | double x2; 37 | double y2; 38 | double z2; 39 | 40 | /*Autotune flex parameter 41 | For Debug Mode. Check the upper and lowe limit of the flex sensors 42 | 3 Flex sensors used. Thumb, Middle, Pinkie 43 | */ 44 | int thumb_high = 0; 45 | int thumb_low = 0; 46 | int finger_high = 0; 47 | int finger_low = 0; 48 | int pinkie_high = 0; 49 | int pinkie_low = 0; 50 | 51 | //Stop Caliberating the Flex Sensor when complete 52 | bool bool_caliberate = false; 53 | 54 | //How often to send values to the Robotic Arm 55 | int response_time = 100; 56 | 57 | void setup() { 58 | pinMode(3, OUTPUT); 59 | Wire.begin(); 60 | Wire.beginTransmission(MPU1); 61 | Wire.write(0x6B);// PWR_MGMT_1 register 62 | Wire.write(0); // set to zero (wakes up the MPU-6050) 63 | Wire.endTransmission(true); Wire.begin(); 64 | Wire.beginTransmission(MPU2); 65 | Wire.write(0x6B);// PWR_MGMT_1 register 66 | Wire.write(0); // set to zero (wakes up the MPU-6050) 67 | Wire.endTransmission(true); 68 | Serial.begin(4800); 69 | delay(1000); 70 | 71 | } 72 | void loop() { 73 | 74 | /* 75 | Note: Serial.print() would send all values the robotic arm using via bluetooth. 76 | */ 77 | pinMode(3, HIGH); //Use basic LED as visual indicator if value being sent 78 | 79 | //debug_flex(); //Debug Mode on/off 80 | //Serial.println("test"); 81 | //get values for first mpu having address of 0x68 82 | GetMpuValue1(MPU1); 83 | //Serial.prinlnt(" "); 84 | delay(10); 85 | 86 | //get values for second mpu having address of 0x69 87 | GetMpuValue2(MPU2); 88 | //Serial.println(""); 89 | delay(10); 90 | 91 | //Print out a value, based on the change of the XYZ co-ordinates of 1st or 2nd MPU 92 | 93 | //Move Left 94 | if ( x > 15 && x < 55 && y < 30) { 95 | Serial.print("L"); 96 | delay(response_time); 97 | } 98 | 99 | //Move Right 100 | if ( x < 310 && x > 270) { 101 | Serial.print("R"); 102 | delay(response_time); 103 | } 104 | 105 | //Claw Up 106 | if ( y > 60 && y < 80) { 107 | Serial.print("G"); 108 | delay(response_time); 109 | } 110 | 111 | // //Claw Down 112 | if ( y < 310 && y > 270) { 113 | Serial.print("U"); 114 | delay(response_time); 115 | } 116 | 117 | // //Move right 118 | if ( y2 > 50 && y2 < 85) { 119 | Serial.print("C"); 120 | delay(response_time); 121 | } 122 | 123 | // //Move left --- Right Hand 124 | if ( y2 < 160 && y2 > 120) { 125 | Serial.print("c"); 126 | delay(response_time); 127 | 128 | } 129 | 130 | // read the values from Flex Sensors to Arduino 131 | pinkie = analogRead(pinkie_Data); 132 | finger = analogRead(finger_Data); 133 | thumb = analogRead(thumb_Data); 134 | 135 | 136 | //Calibrate to find upper and lower limit of the Flex Sensor 137 | if (bool_caliberate == false ) { 138 | delay(1000); 139 | 140 | thumb_high = (thumb * 1.15); 141 | thumb_low = (thumb * 0.9); 142 | 143 | finger_high = (finger * 1.03); 144 | finger_low = (finger * 0.8); 145 | 146 | pinkie_high = (pinkie * 1.06); 147 | pinkie_low = (pinkie * 0.8); 148 | 149 | bool_caliberate = true; 150 | } 151 | 152 | delay(response_time); 153 | 154 | // Pinkie 155 | if (pinkie >= pinkie_high) { 156 | Serial.print("P"); 157 | delay(response_time); 158 | 159 | } 160 | if (pinkie <= pinkie_low ) { 161 | Serial.print("p"); 162 | delay(response_time); 163 | } 164 | 165 | 166 | // thumb 1 - thumb (Base Rotation) 167 | if (thumb >= thumb_high) { 168 | Serial.print("T"); 169 | delay(response_time); 170 | } 171 | 172 | if (thumb <= thumb_low) { 173 | Serial.print("t"); 174 | delay(response_time); 175 | } 176 | 177 | // finger 1 - Claw Bend/Open 178 | if (finger >= finger_high) { 179 | Serial.print("F"); 180 | delay(response_time); 181 | } 182 | 183 | if (finger <= finger_low) { 184 | Serial.print("f"); 185 | delay(response_time); 186 | } 187 | else { 188 | delay(5); 189 | } 190 | } 191 | 192 | void GetMpuValue1(const int MPU) { 193 | 194 | Wire.beginTransmission(MPU); 195 | Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) 196 | Wire.endTransmission(false); 197 | Wire.requestFrom(MPU, 14, true); // request a total of 14 registers 198 | 199 | AcX1 = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) 200 | AcY1 = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) 201 | AcZ1 = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) 202 | 203 | Tmp1 = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) 204 | 205 | int xAng = map(AcX1, minVal, maxVal, -90, 90); 206 | int yAng = map(AcY1, minVal, maxVal, -90, 90); 207 | int zAng = map(AcZ1, minVal, maxVal, -90, 90); 208 | 209 | GyX1 = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) 210 | GyY1 = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) 211 | GyZ1 = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) 212 | 213 | x = RAD_TO_DEG * (atan2(-yAng, -zAng) + PI) + 4; //offset by 4 degrees to get back to zero 214 | y = RAD_TO_DEG * (atan2(-xAng, -zAng) + PI); 215 | z = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI); 216 | 217 | //-- Comment to Debug 218 | // Serial.print("AngleX= "); 219 | // Serial.print(x); 220 | // Serial.print("\t"); 221 | // 222 | // Serial.print("AngleY= "); 223 | // Serial.print(y); 224 | // Serial.print("\t"); 225 | // 226 | // Serial.print("AngleZ= "); 227 | // Serial.print(z); 228 | // Serial.print("\t"); 229 | // Serial.println("-----------------------------------------"); 230 | 231 | 232 | // Serial.print("AcX = "); 233 | // Serial.print(AcX1); 234 | // Serial.print(" | AcY = "); 235 | // Serial.print(AcY1); 236 | // Serial.print(" | AcZ = "); 237 | // Serial.print(AcZ1); 238 | // Serial.print(" | GyX = "); 239 | // Serial.print(GyX1); 240 | // Serial.print(" | GyY = "); 241 | // Serial.print(GyY1); 242 | // Serial.print(" | GyZ = "); 243 | // Serial.println(GyZ1); 244 | } 245 | 246 | void GetMpuValue2(const int MPU) { 247 | 248 | Wire.beginTransmission(MPU); 249 | Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) 250 | Wire.endTransmission(false); 251 | Wire.requestFrom(MPU, 14, true); // request a total of 14 registers 252 | AcX2 = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) 253 | AcY2 = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) 254 | AcZ2 = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) 255 | 256 | Tmp2 = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) 257 | 258 | int xAng2 = map(AcX2, minVal2, maxVal2, -90, 90); 259 | int yAng2 = map(AcY2, minVal2, maxVal2, -90, 90); 260 | int zAng2 = map(AcZ2, minVal2, maxVal2, -90, 90); 261 | 262 | GyX2 = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) 263 | GyY2 = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) 264 | GyZ2 = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) 265 | 266 | x2 = RAD_TO_DEG * (atan2(-yAng2, -zAng2) + PI) + 4; //offset by 4 degrees to get back to zero 267 | y2 = RAD_TO_DEG * (atan2(-xAng2, -zAng2) + PI); 268 | z2 = RAD_TO_DEG * (atan2(-yAng2, -xAng2) + PI); 269 | 270 | //-- Comment to Debug 271 | // Serial.print("AcX = "); 272 | // Serial.print(AcX2); 273 | // Serial.print(" | AcY = "); 274 | // Serial.print(AcY2); 275 | // Serial.print(" | AcZ = "); 276 | // Serial.print(AcZ2); 277 | // Serial.print(" | GyX = "); 278 | // Serial.print(GyX2); 279 | // Serial.print(" | GyY = "); 280 | // Serial.print(GyY2); 281 | // Serial.print(" | GyZ = "); 282 | // Serial.println(GyZ2); 283 | // 284 | // Serial.print("AngleX2= "); 285 | // Serial.print(x2); 286 | // Serial.print("\t"); 287 | // 288 | // Serial.print("AngleY2= "); 289 | // Serial.print(y2); 290 | // Serial.print("\t"); 291 | // 292 | // Serial.print("AngleZ2= "); 293 | // Serial.print(z2); 294 | // Serial.print("\t"); 295 | // Serial.println("-----------------------------------------"); 296 | 297 | } 298 | 299 | void debug_flex() { 300 | //Sends value as a serial monitor to port 301 | //thumb (Claw open / close) 302 | Serial.print("Thumb: "); 303 | Serial.print(thumb); 304 | Serial.print("\t"); 305 | // //thumb Params 306 | Serial.print("thumb High: "); 307 | Serial.print(thumb_high); 308 | Serial.print("\t"); 309 | Serial.print("T Low: "); 310 | Serial.print(thumb_low); 311 | Serial.print("\t"); 312 | 313 | //finger (Claw Further) 314 | Serial.print("finger: "); 315 | Serial.print(finger); 316 | Serial.print("\t"); 317 | 318 | // finger Params 319 | Serial.print("finger High: "); 320 | Serial.print(finger_high); 321 | Serial.print("\t"); 322 | Serial.print("finger Low: "); 323 | Serial.print(finger_low); 324 | Serial.print("\t"); 325 | 326 | //Pinkie (Claw Further) 327 | Serial.print("Pinkie: "); 328 | Serial.print(pinkie); 329 | Serial.print("\t"); 330 | 331 | // //Pinkie Params 332 | Serial.print("Pinkie High: "); 333 | Serial.print(pinkie_high); 334 | Serial.print("\t"); 335 | Serial.print("Pinkie Low: "); 336 | Serial.print(pinkie_low); 337 | Serial.print("\t"); 338 | Serial.println(); 339 | } 340 | -------------------------------------------------------------------------------- /code/test-code/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EbenKouao/arduino-robot-arm/57041c18da89bd6983156740ac52b19ef2cfc468/code/test-code/.DS_Store -------------------------------------------------------------------------------- /code/test-code/flex_sensor_test/flex_sensor_test.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Author: Smartbuilds.io 3 | YouTube: https://www.youtube.com/channel/UCGxwyXJWEarxh2XWqvygiIg 4 | Fork your own version: https://github.com/EbenKouao/arduino-robotic-arm 5 | Date: 28/12/2020 6 | Description: Test the Flex Sensor 7 | ******************************************************************************/ 8 | const int flexPin = A3; // Pin connected to voltage divider output 9 | 10 | void setup() 11 | { 12 | Serial.begin(4800); 13 | pinMode(flexPin, INPUT); 14 | } 15 | 16 | void loop() 17 | { 18 | // Read the ADC, and calculate voltage and resistance from it 19 | int flexSensor = analogRead(flexPin); 20 | 21 | Serial.println(flexSensor); 22 | 23 | delay(50); 24 | } 25 | -------------------------------------------------------------------------------- /code/test-code/mpu6050-degrees-test/mpu6050-degrees-test.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Author: Smartbuilds.io 3 | YouTube: https://www.youtube.com/channel/UCGxwyXJWEarxh2XWqvygiIg 4 | Fork your own version: https://github.com/EbenKouao/arduino-robotic-arm 5 | Date: 28/12/2020 6 | Description: Test the MPU relative position X & Y co-ord in degreed 7 | 8 | ******************************************************************************/ 9 | 10 | #include 11 | 12 | #define button 8 13 | int state = 0; 14 | int buttonState = 0; 15 | int check = 1; 16 | int nil = 0; 17 | 18 | const int MPU2 = 0x69, MPU1 = 0x68; 19 | 20 | int16_t AcX1, AcY1, AcZ1, Tmp1, GyX1, GyY1, GyZ1; 21 | 22 | int minVal = 265; 23 | int maxVal = 402; 24 | 25 | double x; 26 | double y; 27 | double z; 28 | 29 | int response_time = 40; 30 | 31 | void setup() { 32 | pinMode(3, OUTPUT); 33 | Wire.begin(); 34 | Wire.beginTransmission(MPU1); 35 | Wire.write(0x6B);// PWR_MGMT_1 register 36 | Wire.write(0); // set to zero (wakes up the MPU-6050) 37 | Wire.endTransmission(true); Wire.begin(); 38 | Wire.beginTransmission(MPU2); 39 | Wire.write(0x6B);// PWR_MGMT_1 register 40 | Wire.write(0); // set to zero (wakes up the MPU-6050) 41 | Wire.endTransmission(true); 42 | Serial.begin(4800); 43 | delay(1000); 44 | 45 | } 46 | void loop() { 47 | //get values for first mpu having address of 0x68 48 | GetMpuValue1(MPU1); 49 | } 50 | 51 | void GetMpuValue1(const int MPU) { 52 | 53 | Wire.beginTransmission(MPU); 54 | Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) 55 | Wire.endTransmission(false); 56 | Wire.requestFrom(MPU, 14, true); // request a total of 14 registers 57 | 58 | AcX1 = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) 59 | AcY1 = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) 60 | AcZ1 = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) 61 | 62 | Tmp1 = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) 63 | 64 | int xAng = map(AcX1, minVal, maxVal, -90, 90); 65 | int yAng = map(AcY1, minVal, maxVal, -90, 90); 66 | int zAng = map(AcZ1, minVal, maxVal, -90, 90); 67 | 68 | GyX1 = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) 69 | GyY1 = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) 70 | GyZ1 = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) 71 | 72 | x = RAD_TO_DEG * (atan2(-yAng, -zAng) + PI) + 4; //offset by 4 degrees to get back to zero 73 | y = RAD_TO_DEG * (atan2(-xAng, -zAng) + PI); 74 | z = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI); 75 | 76 | Serial.print("AngleX= "); 77 | Serial.print(x); 78 | Serial.print("\t"); 79 | 80 | Serial.print("AngleY= "); 81 | Serial.print(y); 82 | Serial.print("\t"); 83 | 84 | Serial.print("AngleZ= "); 85 | Serial.print(z); 86 | Serial.print("\t"); 87 | Serial.println("-----------------------------------------"); 88 | } 89 | -------------------------------------------------------------------------------- /code/test-code/nema_17_motor_test/nema_17_motor_test.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Author: Smartbuilds.io 3 | YouTube: https://www.youtube.com/channel/UCGxwyXJWEarxh2XWqvygiIg 4 | Fork your own version: https://github.com/EbenKouao/arduino-robotic-arm 5 | Date: 28/12/2020 6 | Description: Test the Stepper Motor 7 | ******************************************************************************/ 8 | 9 | // Define pin connections & motor's steps per revolution 10 | const int dirPin = 4; 11 | const int stepPin = 5; 12 | const int stepsPerRevolution = 200; 13 | int stepDelay = 9500; 14 | void setup() 15 | { 16 | // Declare pins as Outputs 17 | pinMode(stepPin, OUTPUT); 18 | pinMode(dirPin, OUTPUT); 19 | } 20 | void loop() 21 | { 22 | baseRotateLeft(); 23 | baseRotateRight(); 24 | 25 | } 26 | 27 | void baseRotateLeft() { 28 | //clockwise 29 | digitalWrite(dirPin, HIGH); 30 | // Spin motor 31 | for (int x = 0; x < stepsPerRevolution; x++) 32 | { 33 | digitalWrite(stepPin, HIGH); 34 | delayMicroseconds(stepDelay); 35 | digitalWrite(stepPin, LOW); 36 | delayMicroseconds(stepDelay); 37 | } 38 | delay(2000); // Wait a second 39 | } 40 | 41 | 42 | void baseRotateRight() { 43 | 44 | //counterclockwise 45 | digitalWrite(dirPin, LOW); 46 | // Spin motor 47 | for (int x = 0; x < stepsPerRevolution; x++) 48 | { 49 | digitalWrite(stepPin, LOW); 50 | delayMicroseconds(stepDelay); 51 | digitalWrite(stepPin, HIGH); 52 | delayMicroseconds(stepDelay); 53 | } 54 | delay(2000); // Wait a second 55 | } 56 | -------------------------------------------------------------------------------- /code/test-code/servo_motor_single/servo_motor_single.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Author: Smartbuilds.io 3 | YouTube: https://www.youtube.com/channel/UCGxwyXJWEarxh2XWqvygiIg 4 | Fork your own version: https://github.com/EbenKouao/arduino-robotic-arm 5 | Date: 28/12/2020 6 | Description: Test the Single Servo Motor 7 | 8 | Note: To Avoid excess Arduino Power consumption. 9 | Power with external power sourcee if you would would like to add more than 2 servos. 10 | ******************************************************************************/ 11 | 12 | #include 13 | 14 | Servo myservo_1; // create servo object to control a servo 15 | int pos = 0; // variable to store the servo position 16 | 17 | void setup() { 18 | myservo_1.attach(9); // attaches the servo on pin 9 to the servo object 19 | myservo_1.write(90); 20 | delay(5000); 21 | } 22 | 23 | void loop() { 24 | 25 | for (pos = 0; pos <= 80; pos += 1) { // goes from 0 degrees to 180 degrees 26 | // in steps of 1 degree 27 | myservo_1.write(pos); // tell servo to go to position in variable 'pos' 28 | delay(15); // waits 15ms for the servo to reach the position 29 | } 30 | for (pos = 80; pos >= 1; pos -= 1) { // goes from 180 degrees to 0 degrees 31 | myservo_1.write(pos); // tell servo to go to position in variable 'pos' 32 | delay(15); // waits 15ms for the servo to reach the position 33 | } 34 | 35 | 36 | } 37 | --------------------------------------------------------------------------------