Components and supplies
I2C 16x2 Arduino LCD Display Module
4xAA battery holder
SG90 Micro-servo motor
Arduino UNO
Joystick, 10 kohm
Resistor 1k ohm
AA Batteries
MG996R Digital Servo
Tactile Switch, Top Actuated
Breadboard (generic)
HC-05 Bluetooth Module
Resistor 10k ohm
Jumper wires (generic)
Resistor 220 ohm
RESISTOR 2K Ohm
9V battery (generic)
Tools and machines
Hot glue gun (generic)
3D Printer (generic)
Laser cutter (generic)
Apps and platforms
Arduino IDE
Project description
Code
Joystick Code
arduino
1/* 2 Project: Bracc.ino 3 Date: 09/01/2020 4 Last change: 23/01/2020 5 6 Team: Basso Andrea 7 Delmoro Niccol 8 Gramaglia Giacomo 9 //////JOYPAD CODE////// 10*/ 11//////////////////// 12////DECLARATIONS//// 13//////////////////// 14 15/*==LIBRARIES==*/ 16#include <SoftwareSerial.h> 17SoftwareSerial BTserial(2, 3); // HC-05 = 2-RX(+resistors) | 3-TX 18#include <LiquidCrystal_I2C.h> 19LiquidCrystal_I2C lcd (0x27, 16, 2); //sda=A4 , scl=A5 20 21/*==BUTTONS==*/ 22int button1Pin = 4; //Open Gripper 23int valButton1; 24int button2Pin = 5; //Save Point 25int valButton2; 26int button3Pin = 7; //Close Gripper 27int valButton3; 28int button4Pin = 8; //Move sequence 29int valButton4; 30//Joystick Buttons 31int button5Pin = 9; //Coordinate Mode (NC) 32int valButton5; 33int button6Pin = 6; //Home position 34int valButton6; 35 36/*==JOYSTICK 1==*/ 37int joy1xPin = A0; 38int joy1yPin = A1; 39int valX1; 40int valY1; 41 42/*==JOYSTICK 2==*/ 43int joy2xPin = A2; 44int joy2yPin = A3; 45int valX2; 46int valY2; 47 48 49/*==ANGLE VARIABLES==*/ 50//Base 51int base = 90; 52int baseMax = 180; 53int baseMin = 0; 54//Servo1 55int servo1 = 73; 56int servo1Max = 180; 57int servo1Min = 0; 58//Servo2 59int servo2 = 26; 60int servo2Max = 180; 61int servo2Min = 0; 62//Servo3 63int servo3 = 147; 64int servo3Max = 180; 65int servo3Min = 0; 66//Servo4 67int servo4 = 14; 68int servo4Max = 180; 69int servo4Min = 0; 70 71/*==Movement Commands==*/ 72char MDM = '0'; //Turn on direct motor mode 73char MB1 = 'A'; //Move base Clockwise 74char MB2 = 'B'; //Move base Counterclockwise 75char M11 = 'C'; //Move servo1 Clockwise 76char M12 = 'D'; //Move servo1 Counterclockwise 77char M21 = 'E'; //Move servo2 Clockwise 78char M22 = 'F'; //Move servo2 Counterclockwise 79char M31 = 'G'; //Move servo3 Clockwise 80char M32 = 'H'; //Move servo3 Counterclockwise 81char M41 = 'I'; //Move servo4 Clockwise 82char M42 = 'J'; //Move servo4 Counterclockwise 83char MG1 = 'K'; //Open gripper 84char MG2 = 'L'; //Close gripper 85char MHO = 'M'; //Move home point 86char MPS = 'N'; //Move points sequence 87/*==Point Commands==*/ 88char PRD = 'P'; //Reading point 89/*==Coordinate Commands==*/ 90char CMM = '1'; //Turn in coordinate Mode 91char CX1 = 'R'; //Increase X value 92char CX2 = 'S'; //Decrease X value 93char CY1 = 'T'; //Increase Y value 94char CY2 = 'U'; //Decrease Y value 95char CZ1 = 'V'; //Increase Z angle 96char CZ2 = 'W'; //Decrease Z angle 97char CA1 = 'X'; //Grip angle increase-clockwise 98char CA2 = 'Y'; //Grip angle decrease-counterclockwise 99 100/*==TIME VALUE==*/ 101int Speed = 8; 102 103/*==IK VALUES==*/ 104int XYMode; 105int modeTouch; 106int x = 95; 107int y = 80; 108int angle = 90; 109int DB; 110 111//////////////////// 112////////SETUP/////// 113//////////////////// 114 115void setup() 116{ 117 //Button declarations 118 pinMode(button1Pin, INPUT); 119 pinMode(button2Pin, INPUT); 120 pinMode(button3Pin, INPUT); 121 pinMode(button4Pin, INPUT); 122 pinMode(button5Pin, INPUT); 123 pinMode(button6Pin, INPUT); 124 125 //Joysticks declarations 126 pinMode(joy1xPin, INPUT); 127 pinMode(joy1yPin, INPUT); 128 pinMode(joy2xPin, INPUT); 129 pinMode(joy2yPin, INPUT); 130 131 //Begins 132 Serial.begin(9600); 133 BTserial.begin(38400); 134 lcd.init(); 135 lcd.clear(); 136 lcd.backlight(); 137 lcd.setCursor(0, 0); 138 lcd.print("Inizialitation..."); 139} 140 141//////////////////// 142////////LOOP//////// 143//////////////////// 144 145void loop() 146{ 147 valButton5 = digitalRead(button5Pin); 148 149 //Mode Selection 150 if (valButton5 == LOW && modeTouch == LOW) //Enter in IK Mode 151 { 152 XYMode = 1; 153 modeTouch = 1; 154 delay(Speed); 155 lcd.clear(); 156 BTserial.write(CMM); 157 lcd.setCursor(0, 0); 158 lcd.print("Coordinate XY"); 159 lcd.setCursor(0, 1); 160 lcd.print("Mode..."); 161 //Initialitation 162 x = 95; 163 y = 80; 164 angle = 90; 165 base = 90; 166 delay(1000); 167 } 168 else if (valButton5 == LOW && modeTouch == HIGH) //Exit from IK Mode 169 { 170 XYMode = 0; 171 modeTouch = 0; 172 delay(Speed); 173 lcd.clear(); 174 BTserial.write(MDM); 175 lcd.setCursor(0, 0); 176 lcd.print("Direct motor"); 177 lcd.setCursor(0, 1); 178 lcd.print("Mode..."); 179 //Inizialitation 180 base = 90; 181 servo1 = 73; 182 servo2 = 26; 183 servo3 = 147; 184 servo4 = 14; 185 delay(1000); 186 } 187 188 //Mode actions [IK - DIRECT] 189 if (XYMode == 1) //IK Mode 190 { 191 coordinateMode(); 192 } 193 else if (XYMode == 0) //Direct Mode 194 { 195 readMovement(); 196 } 197 reading(); 198} 199 200//////////////////// 201//////READING/////// 202//////////////////// 203 204void reading() 205{ 206 valButton1 = digitalRead(button1Pin); 207 valButton2 = digitalRead(button2Pin); 208 valButton3 = digitalRead(button3Pin); 209 valButton4 = digitalRead(button4Pin); 210 valButton6 = digitalRead(button6Pin); 211 212 /*===== MG1 - MG2 = GRIPPER =====*/ 213 if (valButton1 == HIGH) //LowButtonLeft - Gripper opening 214 { 215 delay(Speed); 216 lcd.clear(); 217 BTserial.write(MG1); 218 lcd.setCursor(0, 0); 219 lcd.print("Gripper ="); 220 lcd.setCursor(0, 1); 221 lcd.print("Opened"); 222 } 223 else if (valButton3 == HIGH) //LowButtonRight- Gripper Closing 224 { 225 delay(Speed); 226 lcd.clear(); 227 BTserial.write(MG2); 228 lcd.setCursor(0, 0); 229 lcd.print("Gripper ="); 230 lcd.setCursor(0, 1); 231 lcd.print("Closed"); 232 } 233 234 /*===== Homing Button =====*/ 235 if (valButton6 == HIGH) //JoystickRightButton - Active Home 236 { 237 delay(Speed); 238 lcd.clear(); 239 BTserial.write(MHO); 240 lcd.setCursor(0, 0); 241 lcd.print("Homing"); 242 base = 90; 243 servo1 = 73; 244 servo2 = 26; 245 servo3 = 147; 246 servo4 = 14; 247 248 x = 95; 249 y = 80; 250 angle = 90; 251 delay(1500); 252 } 253 254 /*===== Points =====*/ 255 if (valButton2 == HIGH) //HighButtonLeft - Save point position 256 { 257 delay(Speed); 258 lcd.clear(); 259 lcd.setCursor(0, 0); 260 BTserial.write(PRD); 261 savePoint(); 262 delay(1500); 263 } 264 if (valButton4 == HIGH) //HighButtonRight - Move points sequence 265 { 266 delay(Speed); 267 lcd.clear(); 268 BTserial.write(MPS); 269 lcd.setCursor(0, 0); 270 lcd.print("Move points"); 271 lcd.setCursor(0, 1); 272 lcd.print("sequence"); 273 delay(1500); 274 } 275} 276 277void limits() //Limit angle between angleMax and angle Min 278{ 279 //base limitation 280 if (base > baseMax) 281 base = baseMax; 282 else if (base < baseMin) 283 base = baseMin; 284 //Servo1 limitation 285 if (servo1 > servo1Max) 286 servo1 = servo1Max; 287 else if (servo1 < servo1Min) 288 servo1 = servo1Min; 289 //Servo2 limitation 290 if (servo2 > servo2Max) 291 servo2 = servo2Max; 292 else if (servo2 < servo2Min) 293 servo2 = servo2Min; 294 //Servo3 limitation 295 if (servo3 > servo3Max) 296 servo3 = servo3Max; 297 else if (servo3 < servo3Min) 298 servo3 = servo3Min; 299 //Servo4 limitation 300 if (servo4 > servo4Max) 301 servo4 = servo4Max; 302 else if (servo4 < servo4Min) 303 servo4 = servo4Min; 304} 305 306//////////////////// 307//////DIRECT//////// 308//////////////////// 309 310void readMovement() 311{ 312 valX1 = analogRead(joy1xPin); 313 valY1 = analogRead(joy1yPin); 314 valX2 = analogRead(joy2xPin); 315 valY2 = analogRead(joy2yPin); 316 317 /*===== MB1 - MB2 = BASE =====*/ 318 if (valX1 > 700 && valX2 > 700) //joy1-2 SenseLeft - Base Clockwise 319 { 320 delay(Speed); 321 lcd.clear(); 322 BTserial.write(MB1); 323 base = base + 1; 324 lcd.setCursor(0, 0); 325 lcd.print("Base Angle="); 326 lcd.setCursor(0, 1); 327 lcd.print(base); 328 } 329 else if (valX1 < 350 && valX2 < 350) //joy1-2 SenseRight - Base Counterclockwise 330 { 331 delay(Speed); 332 lcd.clear(); 333 BTserial.write(MB2); 334 base = base - 1; 335 lcd.setCursor(0, 0); 336 lcd.print("Base Angle="); 337 lcd.setCursor(0, 1); 338 lcd.print(base); 339 } 340 341 /*===== M11 - M12 = SERVO 1 =====*/ 342 if (valY1 > 700 && valY2 > 700) //Joy1-2 SenseDown - Servo1 Clockwise 343 { 344 delay(Speed); 345 lcd.clear(); 346 BTserial.write(M11); 347 servo1 = servo1 + 1; 348 lcd.setCursor(0, 0); 349 lcd.print("Servo1 Angle ="); 350 lcd.setCursor(0, 1); 351 lcd.print(servo1); 352 } 353 else if (valY1 < 350 && valY2 < 350) //Joy1-2 SenseUp - Servo1 Counterclockwise 354 { 355 delay(Speed); 356 lcd.clear(); 357 BTserial.write(M12); 358 servo1 = servo1 - 1; 359 lcd.setCursor(0, 0); 360 lcd.print("Servo1 Angle ="); 361 lcd.setCursor(0, 1); 362 lcd.print(servo1); 363 } 364 365 /*===== M21 - M22 = SERVO 2 =====*/ 366 if (valY1 < 350 && valY2 > 400 && valY2 < 600) //Joy1 SenseDown - Servo2 Clockwise 367 { 368 delay(Speed); 369 lcd.clear(); 370 BTserial.write(M21); 371 servo2 = servo2 + 1; 372 lcd.setCursor(0, 0); 373 lcd.print("Servo2 Angle ="); 374 lcd.setCursor(0, 1); 375 lcd.print(servo2); 376 } 377 else if ( valY1 > 700 && valY2 < 600 && valY2 > 400) //Joy1 SenseUp - Servo2 Counterclockwise 378 { 379 delay(Speed); 380 lcd.clear(); 381 BTserial.write(M22); 382 servo2 = servo2 - 1; 383 lcd.setCursor(0, 0); 384 lcd.print("Servo2 Angle ="); 385 lcd.setCursor(0, 1); 386 lcd.print(servo2); 387 } 388 389 /*===== M31 - M32 = SERVO 3 =====*/ 390 else if (valY2 > 700 && valY1 > 400 && valY1 < 600) //Joy2 SenseDown - Servo3 Clockwise 391 { 392 delay(Speed); 393 lcd.clear(); 394 BTserial.write(M31); 395 servo3 = servo3 + 1; 396 lcd.setCursor(0, 0); 397 lcd.print("Servo3 Angle ="); 398 lcd.setCursor(0, 1); 399 lcd.print(servo3); 400 } 401 else if (valY2 < 350 && valY1 > 400 && valY1 < 600) //Joy2 SenseUp - Servo3 Counterclockwise 402 { 403 delay(Speed); 404 lcd.clear(); 405 BTserial.write(M32); 406 servo3 = servo3 - 1; 407 lcd.setCursor(0, 0); 408 lcd.print("Servo3 Angle ="); 409 lcd.setCursor(0, 1); 410 lcd.print(servo3); 411 } 412 413 /*===== M41 - M42 = SERVO 4 =====*/ 414 if (valX1 > 700 && valX2 < 350) //Joy1 SenseLeft 2 SenseRight - Servo4 Clockwise 415 { 416 delay(Speed); 417 lcd.clear(); 418 BTserial.write(M41); 419 servo4 = servo4 + 1; 420 lcd.setCursor(0, 0); 421 lcd.print("Servo4 Angle ="); 422 lcd.setCursor(0, 1); 423 lcd.print(servo4); 424 } 425 else if (valX1 < 350 && valX2 > 700) //Joy1 SenseRight 2 SenseLeft - Servo4 Counterclockwise 426 { 427 delay(Speed); 428 lcd.clear(); 429 BTserial.write(M42); 430 servo4 = servo4 - 1; 431 lcd.setCursor(0, 0); 432 lcd.print("Servo4 Angle ="); 433 lcd.setCursor(0, 1); 434 lcd.print(servo4); 435 } 436 limits(); 437} 438 439//////////////////// 440//////INVERSE/////// 441//////////////////// 442 443void coordinateMode() 444{ 445 valX1 = analogRead(joy1xPin); 446 valY1 = analogRead(joy1yPin); 447 valX2 = analogRead(joy2xPin); 448 valY2 = analogRead(joy2yPin); 449 450 /*==X VARIATION - JOYSTICK1 RIGHT/LEFT==*/ 451 if (valX1 > 700) //X Increase - right 452 { 453 delay(Speed); 454 BTserial.write(CX1); 455 x = x + 1; 456 lcd.clear(); 457 } 458 else if (valX1 < 350) //X Decrease - left 459 { 460 delay(Speed); 461 BTserial.write(CX2); 462 x = x - 1; 463 lcd.clear(); 464 } 465 466 /*==Y VARIATION - JOYSTICK1 UP/DOWN==*/ 467 if (valY1 > 700) //Y increase - Up 468 { 469 delay(Speed); 470 BTserial.write(CY1); 471 y = y + 1; 472 lcd.clear(); 473 } 474 else if (valY1 < 350) //Y Decrease - Down 475 { 476 delay(Speed); 477 BTserial.write(CY2); 478 y = y - 1; 479 lcd.clear(); 480 } 481 482 /*==Z BASE ROTATION==*/ 483 if (valX2 > 700) //Base rotation clockwise 484 { 485 delay(Speed); 486 BTserial.write(CZ1); 487 base = base + 1; 488 lcd.clear(); 489 } 490 else if (valX2 < 350) //Base rotation counterclockwise 491 { 492 delay(Speed); 493 BTserial.write(CZ2); 494 base = base - 1; 495 lcd.clear(); 496 } 497 498 /*==GRIP ANGLE VARIATION==*/ 499 if (valY2 > 700) //Grip clockwise (+) 500 { 501 delay(Speed); 502 BTserial.write(CA1); 503 angle = angle - 1; 504 lcd.clear(); 505 } 506 else if (valY2 < 350) //Grip counterclockwise (-) 507 { 508 delay(Speed); 509 BTserial.write(CA2); 510 angle = angle - 1; 511 lcd.clear(); 512 } 513 514 lcd.setCursor(0, 0); 515 lcd.print("X"); 516 lcd.setCursor(1, 0); 517 lcd.print(x); 518 lcd.setCursor(6, 0); 519 lcd.print("Y"); 520 lcd.setCursor(7, 0); 521 lcd.print(y); 522 lcd.setCursor(12, 0); 523 lcd.print("Z="); 524 lcd.setCursor(13, 0); 525 lcd.print(base); 526 lcd.setCursor(0, 1); 527 lcd.print("GripAngle="); 528 lcd.setCursor(10, 1); 529 lcd.print(angle); 530} 531 532//////////////////// 533////POINT SAVE////// 534//////////////////// 535 536void savePoint() 537{ 538 if (DB == 0) //Save point1 539 { 540 lcd.print("Saved point 1"); 541 DB = 1; 542 } 543 else if (DB == 1) //Save point2 544 { 545 lcd.print("Saved point 2"); 546 DB = 2; 547 } 548 else if (DB == 2) //Save point3 549 { 550 lcd.print("Saved point 3"); 551 DB = 3; 552 } 553 else if (DB == 3) //Save point4 554 { 555 lcd.print("Saved point 4"); 556 DB = 4; 557 } 558 else if (DB == 4) //Save point5 559 { 560 lcd.print("Saved point 5"); 561 DB = 5; 562 } 563 else if (DB == 5) //Save point Home 564 { 565 lcd.print("Last Point"); 566 lcd.setCursor(0, 1); 567 lcd.print("= Home"); 568 DB = 6; 569 } 570 else if (DB == 6) //Max point allarm 571 { 572 lcd.print("MAX Points!!"); 573 lcd.setCursor(0, 1); 574 lcd.print("click for reset..."); 575 DB = 7; 576 } 577 else if (DB == 7) //Reset poits memories 578 { 579 lcd.print("Reset points"); 580 lcd.setCursor(0, 1); 581 lcd.print("Memories"); 582 DB = 0; 583 } 584} 585
Arm Code
arduino
1/* 2 Project: Bracc.ino 3 Date: 09/01/2020 4 Last change: 20/01/2020 5 6 Team: Basso Andrea 7 Delmoro Niccol 8 Gramaglia Giacomo 9 //////ARM CODE////// 10*/ 11 12////////////////// 13///DECLERATIONS/// 14////////////////// 15 16/*==LIBRARIES==*/ 17#include <VarSpeedServo.h> //Servo library 18#include <SoftwareSerial.h> //Virtual serial communication library, for bluetooth 19SoftwareSerial BTserial(0, 1); //RX (tx hc05) | TX (rx hc05-nodo) 20 21 22/*==SERVO CONFIGURATIONS==*/ 23VarSpeedServo base; //Base Rotation servo 24int basePin = 3; 25int baseAngle = 90; 26const int baseInit = 90; 27const int baseMin = 0; 28const int baseMax = 180; 29 30VarSpeedServo servo1; //Shoulder 1 servo 31int servo1Pin = 5; //Pin attached to the servo 32int servo1Angle = 73; //Angle value of the servo 33const int servo1Init = 73; //Initial angle or in HomeMode 34const int servo1Min = 0; //Minimun angle 35const int servo1Max = 180; //Maximun angle 36 37VarSpeedServo servo2; //Elbow 2 servo 38int servo2Pin = 6; 39int servo2Angle = 26; 40const int servo2Init = 26; 41const int servo2Min = 0; 42const int servo2Max = 180; 43 44VarSpeedServo servo3; //Wrist1 3 servo 45const int servo3Pin = 9; 46int servo3Angle = 147; 47const int servo3Init = 147; 48const int servo3Min = 0; 49const int servo3Max = 180; 50 51VarSpeedServo servo4; //Wrist2 4 Servo 52const int servo4Pin = 10; 53int servo4Angle = 14; 54const int servo4Init = 14; 55const int servo4Min = 0; 56const int servo4Max = 180; 57 58VarSpeedServo gripper; //Gripper servo - SG90 59int gripperPin = 11; 60int gripperAngle = 60; 61int gripperInit = 60; 62int gripperMax = 60; 63int gripperMin = 0; 64 65/*==BLUETOOTH COMMUNICATION COMMAND==*/ 66/*==DIRECT MOVEMENT COMMANDS==*/ 67char MDM = '0'; //Turn on direct motor mode 68char MB1 = 'A'; //Move base Clockwise 69char MB2 = 'B'; //Move base Counterclockwise 70char M11 = 'C'; //Move servo1 Clockwise 71char M12 = 'D'; //Move servo1 Counterclockwise 72char M21 = 'E'; //Move servo2 Clockwise 73char M22 = 'F'; //Move servo2 Counterclockwise 74char M31 = 'G'; //Move servo3 Clockwise 75char M32 = 'H'; //Move servo3 Counterclockwise 76char M41 = 'I'; //Move servo4 Clockwise 77char M42 = 'J'; //Move servo4 Counterclockwise 78char MG1 = 'K'; //Open gripper 79char MG2 = 'L'; //Close gripper 80char MHO = 'M'; //Move home point 81char MPS = 'N'; //Move points sequence 82/*==POINTS COMMAND==*/ 83char PRD = 'P'; //Reading point 84/*==INVERSE KINEMATICS COMMANDS==*/ 85char CMM = '1'; //Turn in coordinate Mode 86char CX1 = 'R'; //Increase X value 87char CX2 = 'S'; //Decrease X value 88char CY1 = 'T'; //Increase Y value 89char CY2 = 'U'; //Decrease Y value 90char CZ1 = 'V'; //Increase Z angle 91char CZ2 = 'W'; //Decrease Z angle 92char CA1 = 'X'; //Grip angle increase-clockwise 93char CA2 = 'Y'; //Grip angle decrease-counterclockwise 94/*==READING==*/ 95char reads = ' '; 96 97/*==TIME AND SPEED==*/ 98int timeUp = 8; //delay time 99int servoSpeed; //Speed servo general value 100int servoSpeedSlow = 15; //Slow movement speed 101int servoSpeedFast = 80; //Fast movement speed 102 103/*==DIRECT POINTS==*/ 104int Dpoint1[6]; //Array for points, each contains servo angles value [base,servo1,servo2,servo3,servo4,gripper] 105int Dpoint2[6]; 106int Dpoint3[6]; 107int Dpoint4[6]; 108int Dpoint5[6]; 109int DB; //Memory for save point 110int timeSeq = 2000; //Time after e sequence 111 112/*==IK VALUES==*/ 113int XYMode; //Value for current mode 114int x; //X cartesian value 115int xInit = 95; //Initial x 116int y; //Y cartesian value 117int yInit = 80; //Initial y 118int gripAngle; //Gripper angle angle, respect the x axis 119int gripAngleInit = 90; //Initiale angle 120int IKpoint1[4]; //Array for points, each contains coordinate values [x,y,z,angle] 121int IKpoint2[4]; 122int IKpoint3[4]; 123int IKpoint4[4]; 124int IKpoint5[4]; 125int DBIK; //Memory for save point 126 127/*==IK CALCULATION VALUES==*/ 128 129//Links length 130int link1 = 90; 131int link2 = 85; 132int link3 = 60; 133int link4 = 142; 134//Y offset between base and first joint 135int offset = 93; 136 137//1-Translation point G' 138int yR; //Y minus offset 139double x2; //G' x coordinate 140double y2; //G' y coordinate 141double angle2; //Complementar Grip Angle 142 143//2-Slope calculation 144double m; //Pendance of the slope 145 146//3-Circle intersacation 147double a; 148double b; 149double c; 150double xB1; 151double xB2; 152double xB; //Intersacation x coordinate 153double yB; //Intersacation y coordinate 154double r; //Lenght of line from origin to B point 155 156//4-Triangle angles 157double alfa; //Opposite triangle angle 158double beta; //B point triangle angle 159double gamma; //Origin triangle angle 160 161//5-Deltoid angles 162double d; //Deltoid diagonal 163double alfa2; //Deltoid different angle 164double delta; //Deltoid equal angle value 165 166//6-Angles values 167double sum; //Sum of the first angles 168double J1; //Joint1 Angle 169double J2; //Joint2 Angle 170double J3; //Joint3 Angle 171double J4; //Joint4 Angle 172 173///////////////// 174//////SETUP////// 175///////////////// 176 177void setup() 178{ 179 //Begins 180 BTserial.begin(38400); 181 Serial.begin(9600); 182 Serial.println("Ready for connection"); 183 184 //Servo attaching 185 base.attach(basePin); 186 servo1.attach(servo1Pin); 187 servo2.attach(servo2Pin); 188 servo3.attach(servo3Pin); 189 servo4.attach(servo4Pin); 190 gripper.attach(gripperPin); 191 192 //Home angle initialitation 193 moveHome(); 194} 195 196///////////////// 197//////LOOP/////// 198///////////////// 199 200void loop() 201{ 202 reading(); 203 limits(); 204 moveAll(); 205} 206 207///////////////// 208/////READING///// 209///////////////// 210 211void reading() 212{ 213 reads = 0; 214 215 //Bluetooth reading 216 if (BTserial.available()) 217 { 218 reads = BTserial.read(); 219 } 220 if (reads == MDM) //DIRECT MODE activation 221 { 222 XYMode = 0; 223 moveHome(); 224 } 225 else if (reads == CMM) //INVERSE MODE activation 226 { 227 XYMode = 1; 228 moveHome(); 229 230 } 231 232 if (XYMode == 0) 233 directMode(); 234 else if (XYMode == 1) 235 coordinateMode(); 236} 237 238void limits() //Limit angle between angleMax and angle Min 239{ 240 //Base limitation 241 if (baseAngle > baseMax) 242 baseAngle = baseMax; 243 else if (baseAngle < baseMin) 244 baseAngle = baseMin; 245 246 //Servo1 limitation 247 if (servo1Angle > servo1Max) 248 servo1Angle = servo1Max; 249 else if (servo1Angle < servo1Min) 250 servo1Angle = servo1Min; 251 252 //Servo2 limitation 253 if (servo2Angle > servo2Max) 254 servo2Angle = servo2Max; 255 else if (servo2Angle < servo2Min) 256 servo2Angle = servo2Min; 257 258 //Servo3 limitation 259 if (servo3Angle > servo3Max) 260 servo3Angle = servo3Max; 261 else if (servo3Angle < servo3Min) 262 servo3Angle = servo3Min; 263 264 //Servo4 limitation 265 if (servo4Angle > servo4Max) 266 servo4Angle = servo4Max; 267 else if (servo4Angle < servo4Min) 268 servo4Angle = servo4Min; 269 270 //Gripper limitation 271 if (gripperAngle > gripperMax) 272 gripperAngle = gripperMax; 273 else if (gripperAngle < gripperMin) 274 gripperAngle = gripperMin; 275} 276 277///////////////// 278//////MOVES////// 279///////////////// 280 281void moveAll() 282{ 283 limits(); 284 //Base Movement 285 base.write(baseAngle, servoSpeed, false); 286 //Servo1 Movement 287 servo1.write(servo1Angle, servoSpeed, false); 288 //Servo2 Movement 289 servo2.write(servo2Angle, servoSpeed, false); 290 //Servo3 Movement 291 servo3.write(servo3Angle, servoSpeed, false); 292 //Servo4 Movement 293 servo4.write(servo4Angle, servoSpeed, false); 294 //Gripper Movement 295 gripper.write(gripperAngle, servoSpeed, false); 296} 297 298void moveHome() 299{ 300 servoSpeed = servoSpeedSlow; 301 if (XYMode == 0) //Direct Home 302 { 303 servoSpeed = servoSpeedSlow; 304 baseAngle = baseInit; 305 servo1Angle = servo1Init; 306 servo2Angle = servo2Init; 307 servo3Angle = servo3Init; 308 servo4Angle = servo4Init; 309 gripperAngle = gripperInit; 310 } 311 else if (XYMode == 1) //Coordinate Home 312 { 313 x = xInit; 314 y = yInit; 315 gripAngle = gripAngleInit; 316 baseAngle = baseInit; 317 IKcalculation(); 318 } 319 moveAll(); 320} 321 322///////////////// 323//////DKMODE///// 324///////////////// 325 326void directMode() 327{ 328 /*=====Base reading=====*/ 329 if (reads == MB1) //Base Clockwise 330 { 331 baseAngle = baseAngle + 1; 332 delay(timeUp); 333 servoSpeed = servoSpeedFast; 334 } 335 else if (reads == MB2) //Base Counterclockwise 336 { 337 baseAngle = baseAngle - 1; 338 delay(timeUp); 339 servoSpeed = servoSpeedFast; 340 } 341 342 /*=====Servo1 reading=====*/ 343 if (reads == M11) //S1 Clockwise 344 { 345 servo1Angle = servo1Angle + 1; 346 delay(timeUp); 347 servoSpeed = servoSpeedFast; 348 } 349 else if (reads == M12) //S1 Counterclockwise 350 { 351 servo1Angle = servo1Angle - 1; 352 delay(timeUp); 353 servoSpeed = servoSpeedFast; 354 } 355 356 /*=====Servo2 reading=====*/ 357 //Servo2 reading 358 if (reads == M21) //S2 Clockwise 359 { 360 servo2Angle = servo2Angle + 1; 361 delay(timeUp); 362 servoSpeed = servoSpeedFast; 363 } 364 else if (reads == M22) //S2 Counterclockwise 365 { 366 servo2Angle = servo2Angle - 1; 367 delay(timeUp); 368 servoSpeed = servoSpeedFast; 369 } 370 371 /*=====Servo3 reading=====*/ 372 if (reads == M31) //S3 Clockwise 373 { 374 servo3Angle = servo3Angle + 1; 375 delay(timeUp); 376 servoSpeed = servoSpeedFast; 377 } 378 else if (reads == M32) //S3 Counterclockwise 379 { 380 servo3Angle = servo3Angle - 1; 381 delay(timeUp); 382 servoSpeed = servoSpeedFast; 383 } 384 385 /*=====Servo4 reading=====*/ 386 if (reads == M41) //S4 Clockwise 387 { 388 servo4Angle = servo4Angle + 1; 389 delay(timeUp); 390 servoSpeed = servoSpeedFast; 391 } 392 else if (reads == M42) //S4 Counterclockwise 393 { 394 servo4Angle = servo4Angle - 1; 395 delay(timeUp); 396 servoSpeed = servoSpeedFast; 397 } 398 399 /*=====Gripper reading=====*/ 400 if (reads == MG1) //Opening 401 { 402 gripperAngle = 60; 403 delay(timeUp); 404 servoSpeed = servoSpeedFast; 405 } 406 else if (reads == MG2) //Closing 407 { 408 gripperAngle = 0; 409 delay(timeUp); 410 servoSpeed = servoSpeedFast; 411 } 412 413 /*=====Homing reading=====*/ 414 if (reads == MHO) //Set angles to home position 415 { 416 delay(timeUp); 417 moveHome(); 418 } 419 420 /*=====Reading points DIRECT=====*/ 421 if (reads == PRD) //Points reading 422 { 423 savePoint(); 424 } 425 426 /*=====Points sequence DIRECT=====*/ 427 if (reads == MPS) 428 { 429 moveSequence(); 430 } 431} 432 433//////////////////POINTS DIRECT/////////////////////////// 434 435void savePoint() 436{ 437 if (DB == 0) //Save point1 438 { 439 Dpoint1[0] = base.read(); 440 Dpoint1[1] = servo1.read(); 441 Dpoint1[2] = servo2.read(); 442 Dpoint1[3] = servo3.read(); 443 Dpoint1[4] = servo4.read(); 444 Dpoint1[5] = gripper.read(); 445 DB = 1; 446 } 447 else if (DB == 1) //Save point2 448 { 449 Dpoint2[0] = base.read(); 450 Dpoint2[1] = servo1.read(); 451 Dpoint2[2] = servo2.read(); 452 Dpoint2[3] = servo3.read(); 453 Dpoint2[4] = servo4.read(); 454 Dpoint2[5] = gripper.read(); 455 DB = 2; 456 } 457 else if (DB == 2) //Save point3 458 { 459 Dpoint3[0] = base.read(); 460 Dpoint3[1] = servo1.read(); 461 Dpoint3[2] = servo2.read(); 462 Dpoint3[3] = servo3.read(); 463 Dpoint3[4] = servo4.read(); 464 Dpoint3[5] = gripper.read(); 465 DB = 3; 466 } 467 else if (DB == 3) //Save point4 468 { 469 Dpoint4[0] = base.read(); 470 Dpoint4[1] = servo1.read(); 471 Dpoint4[2] = servo2.read(); 472 Dpoint4[3] = servo3.read(); 473 Dpoint4[4] = servo4.read(); 474 Dpoint4[5] = gripper.read(); 475 DB = 4; 476 } 477 else if (DB == 4) //Save point5 478 { 479 Dpoint5[0] = base.read(); 480 Dpoint5[1] = servo1.read(); 481 Dpoint5[2] = servo2.read(); 482 Dpoint5[3] = servo3.read(); 483 Dpoint5[4] = servo4.read(); 484 Dpoint5[5] = gripper.read(); 485 DB = 5; 486 } 487 else if (DB == 5) //Save Point Home 488 { 489 DB = 6; 490 } 491 else if (DB == 6) //Max point allert 492 { 493 delay(1000); 494 } 495 else if (DB == 7) //Reset Points 496 { 497 DB == 0; 498 } 499} 500 501void moveSequence() 502{ 503 servoSpeed = servoSpeedSlow; 504 if (DB == 6 || DB == 7) //5 points sequence + Home 505 { 506 Point1(); 507 moveAll(); 508 delay(timeSeq); 509 Point2(); 510 moveAll(); 511 delay(timeSeq); 512 Point3(); 513 moveAll(); 514 delay(timeSeq); 515 Point4(); 516 moveAll(); 517 delay(timeSeq); 518 Point5(); 519 moveAll(); 520 delay(timeSeq); 521 moveHome(); 522 delay(timeSeq); 523 } 524 else if (DB == 5) //5 points sequence 525 { 526 Point1(); 527 moveAll(); 528 delay(timeSeq); 529 Point2(); 530 moveAll(); 531 delay(timeSeq); 532 Point3(); 533 moveAll(); 534 delay(timeSeq); 535 Point4(); 536 moveAll(); 537 delay(timeSeq); 538 Point5(); 539 moveAll(); 540 delay(timeSeq); 541 } 542 else if (DB == 4) //4 points sequence 543 { 544 Point1(); 545 moveAll(); 546 delay(timeSeq); 547 Point2(); 548 moveAll(); 549 delay(timeSeq); 550 Point3(); 551 moveAll(); 552 delay(timeSeq); 553 Point4(); 554 moveAll(); 555 delay(timeSeq); 556 } 557 else if (DB == 3) //3 points sequence 558 { 559 Point1(); 560 moveAll(); 561 delay(timeSeq); 562 Point2(); 563 moveAll(); 564 delay(timeSeq); 565 Point3(); 566 moveAll(); 567 delay(timeSeq); 568 } 569 else if (DB == 2) //2 points sequence 570 { 571 Point1(); 572 moveAll(); 573 delay(timeSeq); 574 Point2(); 575 moveAll(); 576 delay(timeSeq); 577 } 578 else if (DB == 1) //1 point sequence 579 { 580 Point1(); 581 moveAll(); 582 delay(timeSeq); 583 } 584} 585 586void Point1() 587{ 588 baseAngle = Dpoint1[0]; 589 servo1Angle = Dpoint1[1]; 590 servo2Angle = Dpoint1[2]; 591 servo3Angle = Dpoint1[3]; 592 servo4Angle = Dpoint1[4]; 593 gripperAngle = Dpoint1[5]; 594} 595void Point2() 596{ 597 baseAngle = Dpoint2[0]; 598 servo1Angle = Dpoint2[1]; 599 servo2Angle = Dpoint2[2]; 600 servo3Angle = Dpoint2[3]; 601 servo4Angle = Dpoint2[4]; 602 gripperAngle = Dpoint2[5]; 603} 604void Point3() 605{ 606 baseAngle = Dpoint3[0]; 607 servo1Angle = Dpoint3[1]; 608 servo2Angle = Dpoint3[2]; 609 servo3Angle = Dpoint3[3]; 610 servo4Angle = Dpoint3[4]; 611 gripperAngle = Dpoint3[5]; 612} 613void Point4() 614{ 615 baseAngle = Dpoint4[0]; 616 servo1Angle = Dpoint4[1]; 617 servo2Angle = Dpoint4[2]; 618 servo3Angle = Dpoint4[3]; 619 servo4Angle = Dpoint4[4]; 620 gripperAngle = Dpoint4[5]; 621} 622void Point5() 623{ 624 baseAngle = Dpoint5[0]; 625 servo1Angle = Dpoint5[1]; 626 servo2Angle = Dpoint5[2]; 627 servo3Angle = Dpoint5[3]; 628 servo4Angle = Dpoint5[4]; 629 gripperAngle = Dpoint5[5]; 630} 631 632///////////////// 633//////IKMODE///// 634///////////////// 635 636void coordinateMode() 637{ 638 /*=====X VALUE VARIATION=====*/ 639 if (reads == CX1) //X increase 640 { 641 x = x + 1; 642 delay(timeUp); 643 servoSpeed = servoSpeedFast; 644 } 645 else if (reads == CX2) //X decrease 646 { 647 x = x - 1; 648 delay(timeUp); 649 servoSpeed = servoSpeedFast; 650 } 651 652 /*=====Y VALUE VARIATION=====*/ 653 if (reads == CY1) //Y increase 654 { 655 y = y + 1; 656 delay(timeUp); 657 servoSpeed = servoSpeedFast; 658 } 659 else if (reads == CY2) //Y decrease 660 { 661 y = y - 1; 662 delay(timeUp); 663 servoSpeed = servoSpeedFast; 664 } 665 666 /*=====Z ROTATION VARIATION=====*/ 667 if (reads == CZ1) //Z clockwise rotation 668 { 669 baseAngle = baseAngle + 1; 670 delay(timeUp); 671 servoSpeed = servoSpeedFast; 672 } 673 else if (reads == CZ2) //Z counterclockwise rotation 674 { 675 baseAngle = baseAngle - 1; 676 delay(timeUp); 677 servoSpeed = servoSpeedFast; 678 } 679 680 /*=====GRIP ANGLE VARIATION=====*/ 681 if (reads == CA1) //GripAngle increase 682 { 683 gripAngle = gripAngle + 1; 684 delay(timeUp); 685 servoSpeed = servoSpeedFast; 686 } 687 else if (reads == CA2) //GripAngle decrease 688 { 689 gripAngle = gripAngle - 1; 690 delay(timeUp); 691 servoSpeed = servoSpeedFast; 692 } 693 694 /*=====HOME POSITION IK=====*/ 695 if (reads == MHO) 696 { 697 moveHome(); 698 } 699 700 /*=====GRIPPER READING=====*/ 701 if (reads == MG1) //Opening 702 { 703 gripperAngle = 60; 704 delay(timeUp); 705 servoSpeed = servoSpeedFast; 706 } 707 else if (reads == MG2) //Closing 708 { 709 gripperAngle = 0; 710 delay(timeUp); 711 servoSpeed = servoSpeedFast; 712 } 713 714 /*=====IK READING POINTS=====*/ 715 if (reads == PRD) 716 { 717 savePointIK(); 718 } 719 if (reads == MPS) 720 { 721 moveSequenceIK(); 722 } 723 IKcalculation(); 724} 725 726//////////////////POINTS INVERSE/////////////////////////// 727void savePointIK() 728{ 729 if (DBIK == 0) //Save point1 730 { 731 IKpoint1[0] = x; 732 IKpoint1[1] = y; 733 IKpoint1[2] = gripAngle; 734 IKpoint1[3] = base.read(); 735 DBIK = 1; 736 } 737 else if (DBIK == 1) //Save point2 738 { 739 IKpoint1[0] = x; 740 IKpoint1[1] = y; 741 IKpoint1[2] = gripAngle; 742 IKpoint1[3] = base.read(); 743 DBIK = 2; 744 } 745 else if (DBIK == 2) //Save point3 746 { 747 IKpoint1[0] = x; 748 IKpoint1[1] = y; 749 IKpoint1[2] = gripAngle; 750 IKpoint1[3] = base.read(); 751 DBIK = 3; 752 } 753 else if (DBIK == 3) //Save point4 754 { 755 IKpoint1[0] = x; 756 IKpoint1[1] = y; 757 IKpoint1[2] = gripAngle; 758 IKpoint1[3] = base.read(); 759 DBIK = 4; 760 } 761 else if (DBIK == 4) //Save point5 762 { 763 IKpoint1[0] = x; 764 IKpoint1[1] = y; 765 IKpoint1[2] = gripAngle; 766 IKpoint1[3] = base.read(); 767 DBIK = 5; 768 } 769 else if (DBIK == 5) //Save point Home at last 770 { 771 DBIK = 6; 772 } 773 else if (DBIK == 6) //Max point allert 774 { 775 DBIK = 7; 776 delay(500); 777 } 778 else if (DBIK == 7) //Reset points 779 { 780 DBIK = 0; 781 delay(1000); 782 } 783} 784 785void moveSequenceIK() 786{ 787 servoSpeed = servoSpeedSlow; 788 if (DBIK == 7 || DBIK == 6) //Point 1-2-3-4-5-Home Sequence 789 { 790 IKPoint1(); 791 delay(timeSeq); 792 IKPoint2(); 793 delay(timeSeq); 794 IKPoint3(); 795 delay(timeSeq); 796 IKPoint4(); 797 delay(timeSeq); 798 IKPoint5(); 799 delay(timeSeq); 800 moveHome(); 801 delay(timeSeq); 802 } 803 else if (DBIK == 5) //Point 1-2-3-4-5 Sequence 804 { 805 IKPoint1(); 806 delay(timeSeq); 807 IKPoint2(); 808 delay(timeSeq); 809 IKPoint3(); 810 delay(timeSeq); 811 IKPoint4(); 812 delay(timeSeq); 813 IKPoint5(); 814 delay(timeSeq); 815 } 816 else if (DBIK == 4) //Point 1-2-3-4 Sequence 817 { 818 IKPoint1(); 819 delay(timeSeq); 820 IKPoint2(); 821 delay(timeSeq); 822 IKPoint3(); 823 delay(timeSeq); 824 IKPoint4(); 825 delay(timeSeq); 826 } 827 else if (DBIK == 3) //Point 1-2-3 Sequence 828 { 829 IKPoint1(); 830 delay(timeSeq); 831 IKPoint2(); 832 delay(timeSeq); 833 IKPoint3(); 834 delay(timeSeq); 835 } 836 else if (DBIK == 2) //Point 1-2 Sequence 837 { 838 IKPoint1(); 839 delay(timeSeq); 840 IKPoint2(); 841 delay(timeSeq); 842 } 843 else if (DBIK == 1) //Point 1 Sequence 844 { 845 IKPoint1(); 846 delay(timeSeq); 847 } 848} 849 850int Vx; 851int Vy; 852double pendance; 853int Vz; 854int Vangle; 855double steps; 856double q; 857 858void IKPoint1() 859{ 860 Vx = IKpoint1[0] - x; 861 Vy = IKpoint1[1] - y; 862 Vangle = IKpoint1[2] - gripAngle; 863 Vz = IKpoint1[3] - baseAngle; 864 865 pendance = Vy / Vx; 866 q = y - (pendance * x); 867 868 if (Vx > 0) 869 { 870 for (steps = 0; steps < Vx; steps++) 871 { 872 x++; 873 y = (pendance * x) + q; 874 if (Vz > 0) 875 if (baseAngle < IKpoint1[3]) 876 baseAngle++; 877 if (Vz < 0) 878 if (baseAngle > IKpoint1[3]) 879 baseAngle--; 880 if (Vangle > 0) 881 if (gripAngle < IKpoint1[2]) 882 gripAngle++; 883 if (Vangle < 0) 884 if (gripAngle > IKpoint1[2]) 885 gripAngle--; 886 delay(100); 887 IKcalculation(); 888 moveAll(); 889 } 890 } 891 if (Vx < 0) 892 { 893 for (steps = Vx; steps > 0; steps--) 894 { 895 x--; 896 y = (pendance * x) + q; 897 if (Vz > 0) 898 if (baseAngle < IKpoint1[3]) 899 baseAngle++; 900 if (Vz < 0) 901 if (baseAngle > IKpoint1[3]) 902 baseAngle--; 903 if (Vangle > 0) 904 if (gripAngle < IKpoint1[2]) 905 gripAngle++; 906 if (Vangle < 0) 907 if (gripAngle > IKpoint1[2]) 908 gripAngle--; 909 delay(100); 910 IKcalculation(); 911 moveAll(); 912 } 913 } 914} 915void IKPoint2() 916{ 917 Vx = IKpoint2[0] - x; 918 Vy = IKpoint2[1] - y; 919 Vangle = IKpoint2[2] - gripAngle; 920 Vz = IKpoint2[3] - baseAngle; 921 922 pendance = Vy / Vx; 923 q = y - (pendance * x); 924 925 if (Vx > 0) 926 { 927 for (steps = 0; steps < Vx; steps++) 928 { 929 x++; 930 y = (pendance * x) + q; 931 if (Vz > 0) 932 if (baseAngle < IKpoint2[3]) 933 baseAngle++; 934 if (Vz < 0) 935 if (baseAngle > IKpoint2[3]) 936 baseAngle--; 937 if (Vangle > 0) 938 if (gripAngle < IKpoint2[2]) 939 gripAngle++; 940 if (Vangle < 0) 941 if (gripAngle > IKpoint2[2]) 942 gripAngle--; 943 delay(100); 944 IKcalculation(); 945 moveAll(); 946 } 947 } 948 if (Vx < 0) 949 { 950 for (steps = Vx; steps > 0; steps--) 951 { 952 x--; 953 y = (pendance * x) + q; 954 if (Vz > 0) 955 if (baseAngle < IKpoint2[3]) 956 baseAngle++; 957 if (Vz < 0) 958 if (baseAngle > IKpoint2[3]) 959 baseAngle--; 960 if (Vangle > 0) 961 if (gripAngle < IKpoint2[2]) 962 gripAngle++; 963 if (Vangle < 0) 964 if (gripAngle > IKpoint2[2]) 965 gripAngle--; 966 delay(100); 967 IKcalculation(); 968 moveAll(); 969 } 970 } 971} 972void IKPoint3() 973{ 974 Vx = IKpoint3[0] - x; 975 Vy = IKpoint3[1] - y; 976 Vangle = IKpoint3[2] - gripAngle; 977 Vz = IKpoint3[3] - baseAngle; 978 979 pendance = Vy / Vx; 980 q = y - (pendance * x); 981 982 if (Vx > 0) 983 { 984 for (steps = 0; steps < Vx; steps++) 985 { 986 x++; 987 y = (pendance * x) + q; 988 if (Vz > 0) 989 if (baseAngle < IKpoint3[3]) 990 baseAngle++; 991 if (Vz < 0) 992 if (baseAngle > IKpoint3[3]) 993 baseAngle--; 994 if (Vangle > 0) 995 if (gripAngle < IKpoint3[2]) 996 gripAngle++; 997 if (Vangle < 0) 998 if (gripAngle > IKpoint3[2]) 999 gripAngle--; 1000 delay(100); 1001 IKcalculation(); 1002 moveAll(); 1003 } 1004 } 1005 if (Vx < 0) 1006 { 1007 for (steps = Vx; steps > 0; steps--) 1008 { 1009 x--; 1010 y = (pendance * x) + q; 1011 if (Vz > 0) 1012 if (baseAngle < IKpoint3[3]) 1013 baseAngle++; 1014 if (Vz < 0) 1015 if (baseAngle > IKpoint3[3]) 1016 baseAngle--; 1017 if (Vangle > 0) 1018 if (gripAngle < IKpoint3[2]) 1019 gripAngle++; 1020 if (Vangle < 0) 1021 if (gripAngle > IKpoint3[2]) 1022 gripAngle--; 1023 delay(100); 1024 IKcalculation(); 1025 moveAll(); 1026 } 1027 } 1028} 1029void IKPoint4() 1030{ 1031 Vx = IKpoint4[0] - x; 1032 Vy = IKpoint4[1] - y; 1033 Vangle = IKpoint4[2] - gripAngle; 1034 Vz = IKpoint4[3] - baseAngle; 1035 1036 pendance = Vy / Vx; 1037 q = y - (pendance * x); 1038 1039 if (Vx > 0) 1040 { 1041 for (steps = 0; steps < Vx; steps++) 1042 { 1043 x++; 1044 y = (pendance * x) + q; 1045 if (Vz > 0) 1046 if (baseAngle < IKpoint4[3]) 1047 baseAngle++; 1048 if (Vz < 0) 1049 if (baseAngle > IKpoint4[3]) 1050 baseAngle--; 1051 if (Vangle > 0) 1052 if (gripAngle < IKpoint4[2]) 1053 gripAngle++; 1054 if (Vangle < 0) 1055 if (gripAngle > IKpoint4[2]) 1056 gripAngle--; 1057 delay(100); 1058 IKcalculation(); 1059 moveAll(); 1060 } 1061 } 1062 if (Vx < 0) 1063 { 1064 for (steps = Vx; steps > 0; steps--) 1065 { 1066 x--; 1067 y = (pendance * x) + q; 1068 if (Vz > 0) 1069 if (baseAngle < IKpoint4[3]) 1070 baseAngle++; 1071 if (Vz < 0) 1072 if (baseAngle > IKpoint4[3]) 1073 baseAngle--; 1074 if (Vangle > 0) 1075 if (gripAngle < IKpoint4[2]) 1076 gripAngle++; 1077 if (Vangle < 0) 1078 if (gripAngle > IKpoint4[2]) 1079 gripAngle--; 1080 delay(100); 1081 IKcalculation(); 1082 moveAll(); 1083 } 1084 } 1085} 1086void IKPoint5() 1087{ 1088 Vx = IKpoint5[0] - x; 1089 Vy = IKpoint5[1] - y; 1090 Vangle = IKpoint5[2] - gripAngle; 1091 Vz = IKpoint5[3] - baseAngle; 1092 1093 pendance = Vy / Vx; 1094 q = y - (pendance * x); 1095 1096 if (Vx > 0) 1097 { 1098 for (steps = 0; steps < Vx; steps++) 1099 { 1100 x++; 1101 y = (pendance * x) + q; 1102 if (Vz > 0) 1103 if (baseAngle < IKpoint5[3]) 1104 baseAngle++; 1105 if (Vz < 0) 1106 if (baseAngle > IKpoint5[3]) 1107 baseAngle--; 1108 if (Vangle > 0) 1109 if (gripAngle < IKpoint5[2]) 1110 gripAngle++; 1111 if (Vangle < 0) 1112 if (gripAngle > IKpoint5[2]) 1113 gripAngle--; 1114 delay(100); 1115 IKcalculation(); 1116 moveAll(); 1117 } 1118 } 1119 if (Vx < 0) 1120 { 1121 for (steps = Vx; steps > 0; steps--) 1122 { 1123 x--; 1124 y = (pendance * x) + q; 1125 if (Vz > 0) 1126 if (baseAngle < IKpoint5[3]) 1127 baseAngle++; 1128 if (Vz < 0) 1129 if (baseAngle > IKpoint5[3]) 1130 baseAngle--; 1131 if (Vangle > 0) 1132 if (gripAngle < IKpoint5[2]) 1133 gripAngle++; 1134 if (Vangle < 0) 1135 if (gripAngle > IKpoint5[2]) 1136 gripAngle--; 1137 delay(100); 1138 IKcalculation(); 1139 moveAll(); 1140 } 1141 } 1142} 1143 1144////////////////// 1145//IK CALCULATION// 1146////////////////// 1147 1148void IKcalculation() 1149{ 1150 yR = y - offset; 1151 1152 //1- Translation to G' point 1153 angle2 = 180 - gripAngle; 1154 x2 = x + (link4 * cos(angle2 * (PI / 180.00))); 1155 y2 = yR + (link4 * sin(angle2 * (PI / 180.00))); 1156 1157 //2- Slope calculation 1158 m = y2 / x2; 1159 1160 //3- Circle intersacation 1161 a = 1.00 + sq(m); 1162 b = -(2.00 * x2) - (2.00 * m * y2); 1163 c = sq(x2) + sq(y2) - sq(link3); 1164 xB1 = (-b + sqrt(sq(b) - (4 * a * c))) / (2.00 * a); 1165 xB2 = (-b - sqrt(sq(b) - (4 * a * c))) / (2.00 * a); 1166 xB = min(xB1, xB2); 1167 yB = m * xB; 1168 r = sqrt(sq(xB) + sq(yB)); 1169 1170 //4- Triangle angles 1171 alfa = (acos((sq(link1) + sq(link2) - sq(r)) / (2.00 * link1 * link2))) * (180.00 / PI); 1172 beta = (acos((sq(link2) + sq(r) - sq(link1)) / (2.00 * link2 * r))) * (180.00 / PI); 1173 gamma = (acos((sq(link1) + sq(r) - sq(link2)) / (2.00 * link1 * r))) * (180.00 / PI); 1174 1175 //5- Deltoid angles 1176 delta = (180.00 - beta) * (PI / 180.00); 1177 d = sqrt(sq(link2) + sq(link3) - (2.00 * link2 * link3 * cos(delta))); 1178 alfa2 = 2.00 * ((asin((link3 * sin(delta)) / d)) * (180.00 / PI)); 1179 1180 //6- Joints angles calculation 1181 J1 = gamma + ((atan(y2 / x2)) * (180.00 / PI)); 1182 J2 = alfa - 90.00 + alfa2; 1183 J3 = delta * (180.00 / PI) - 90.00; 1184 sum = J1 + alfa + alfa2 + delta * (180 / PI); 1185 J4 = 270 + angle2 - sum; 1186 1187 //7- Servo angles convertion 1188 servo1Angle = 180 - J1; 1189 servo2Angle = J2; 1190 servo3Angle = 180 - J3; 1191 servo4Angle = J4; 1192} 1193
Arm Code
arduino
1/* 2 Project: Bracc.ino 3 Date: 09/01/2020 4 Last change: 20/01/2020 5 6 Team: Basso Andrea 7 Delmoro Niccol 8 Gramaglia Giacomo 9 //////ARM CODE////// 10*/ 11 12////////////////// 13///DECLERATIONS/// 14////////////////// 15 16/*==LIBRARIES==*/ 17#include <VarSpeedServo.h> //Servo library 18#include <SoftwareSerial.h> //Virtual serial communication library, for bluetooth 19SoftwareSerial BTserial(0, 1); //RX (tx hc05) | TX (rx hc05-nodo) 20 21 22/*==SERVO CONFIGURATIONS==*/ 23VarSpeedServo base; //Base Rotation servo 24int basePin = 3; 25int baseAngle = 90; 26const int baseInit = 90; 27const int baseMin = 0; 28const int baseMax = 180; 29 30VarSpeedServo servo1; //Shoulder 1 servo 31int servo1Pin = 5; //Pin attached to the servo 32int servo1Angle = 73; //Angle value of the servo 33const int servo1Init = 73; //Initial angle or in HomeMode 34const int servo1Min = 0; //Minimun angle 35const int servo1Max = 180; //Maximun angle 36 37VarSpeedServo servo2; //Elbow 2 servo 38int servo2Pin = 6; 39int servo2Angle = 26; 40const int servo2Init = 26; 41const int servo2Min = 0; 42const int servo2Max = 180; 43 44VarSpeedServo servo3; //Wrist1 3 servo 45const int servo3Pin = 9; 46int servo3Angle = 147; 47const int servo3Init = 147; 48const int servo3Min = 0; 49const int servo3Max = 180; 50 51VarSpeedServo servo4; //Wrist2 4 Servo 52const int servo4Pin = 10; 53int servo4Angle = 14; 54const int servo4Init = 14; 55const int servo4Min = 0; 56const int servo4Max = 180; 57 58VarSpeedServo gripper; //Gripper servo - SG90 59int gripperPin = 11; 60int gripperAngle = 60; 61int gripperInit = 60; 62int gripperMax = 60; 63int gripperMin = 0; 64 65/*==BLUETOOTH COMMUNICATION COMMAND==*/ 66/*==DIRECT MOVEMENT COMMANDS==*/ 67char MDM = '0'; //Turn on direct motor mode 68char MB1 = 'A'; //Move base Clockwise 69char MB2 = 'B'; //Move base Counterclockwise 70char M11 = 'C'; //Move servo1 Clockwise 71char M12 = 'D'; //Move servo1 Counterclockwise 72char M21 = 'E'; //Move servo2 Clockwise 73char M22 = 'F'; //Move servo2 Counterclockwise 74char M31 = 'G'; //Move servo3 Clockwise 75char M32 = 'H'; //Move servo3 Counterclockwise 76char M41 = 'I'; //Move servo4 Clockwise 77char M42 = 'J'; //Move servo4 Counterclockwise 78char MG1 = 'K'; //Open gripper 79char MG2 = 'L'; //Close gripper 80char MHO = 'M'; //Move home point 81char MPS = 'N'; //Move points sequence 82/*==POINTS COMMAND==*/ 83char PRD = 'P'; //Reading point 84/*==INVERSE KINEMATICS COMMANDS==*/ 85char CMM = '1'; //Turn in coordinate Mode 86char CX1 = 'R'; //Increase X value 87char CX2 = 'S'; //Decrease X value 88char CY1 = 'T'; //Increase Y value 89char CY2 = 'U'; //Decrease Y value 90char CZ1 = 'V'; //Increase Z angle 91char CZ2 = 'W'; //Decrease Z angle 92char CA1 = 'X'; //Grip angle increase-clockwise 93char CA2 = 'Y'; //Grip angle decrease-counterclockwise 94/*==READING==*/ 95char reads = ' '; 96 97/*==TIME AND SPEED==*/ 98int timeUp = 8; //delay time 99int servoSpeed; //Speed servo general value 100int servoSpeedSlow = 15; //Slow movement speed 101int servoSpeedFast = 80; //Fast movement speed 102 103/*==DIRECT POINTS==*/ 104int Dpoint1[6]; //Array for points, each contains servo angles value [base,servo1,servo2,servo3,servo4,gripper] 105int Dpoint2[6]; 106int Dpoint3[6]; 107int Dpoint4[6]; 108int Dpoint5[6]; 109int DB; //Memory for save point 110int timeSeq = 2000; //Time after e sequence 111 112/*==IK VALUES==*/ 113int XYMode; //Value for current mode 114int x; //X cartesian value 115int xInit = 95; //Initial x 116int y; //Y cartesian value 117int yInit = 80; //Initial y 118int gripAngle; //Gripper angle angle, respect the x axis 119int gripAngleInit = 90; //Initiale angle 120int IKpoint1[4]; //Array for points, each contains coordinate values [x,y,z,angle] 121int IKpoint2[4]; 122int IKpoint3[4]; 123int IKpoint4[4]; 124int IKpoint5[4]; 125int DBIK; //Memory for save point 126 127/*==IK CALCULATION VALUES==*/ 128 129//Links length 130int link1 = 90; 131int link2 = 85; 132int link3 = 60; 133int link4 = 142; 134//Y offset between base and first joint 135int offset = 93; 136 137//1-Translation point G' 138int yR; //Y minus offset 139double x2; //G' x coordinate 140double y2; //G' y coordinate 141double angle2; //Complementar Grip Angle 142 143//2-Slope calculation 144double m; //Pendance of the slope 145 146//3-Circle intersacation 147double a; 148double b; 149double c; 150double xB1; 151double xB2; 152double xB; //Intersacation x coordinate 153double yB; //Intersacation y coordinate 154double r; //Lenght of line from origin to B point 155 156//4-Triangle angles 157double alfa; //Opposite triangle angle 158double beta; //B point triangle angle 159double gamma; //Origin triangle angle 160 161//5-Deltoid angles 162double d; //Deltoid diagonal 163double alfa2; //Deltoid different angle 164double delta; //Deltoid equal angle value 165 166//6-Angles values 167double sum; //Sum of the first angles 168double J1; //Joint1 Angle 169double J2; //Joint2 Angle 170double J3; //Joint3 Angle 171double J4; //Joint4 Angle 172 173///////////////// 174//////SETUP////// 175///////////////// 176 177void setup() 178{ 179 //Begins 180 BTserial.begin(38400); 181 Serial.begin(9600); 182 Serial.println("Ready for connection"); 183 184 //Servo attaching 185 base.attach(basePin); 186 servo1.attach(servo1Pin); 187 servo2.attach(servo2Pin); 188 servo3.attach(servo3Pin); 189 servo4.attach(servo4Pin); 190 gripper.attach(gripperPin); 191 192 //Home angle initialitation 193 moveHome(); 194} 195 196///////////////// 197//////LOOP/////// 198///////////////// 199 200void loop() 201{ 202 reading(); 203 limits(); 204 moveAll(); 205} 206 207///////////////// 208/////READING///// 209///////////////// 210 211void reading() 212{ 213 reads = 0; 214 215 //Bluetooth reading 216 if (BTserial.available()) 217 { 218 reads = BTserial.read(); 219 } 220 if (reads == MDM) //DIRECT MODE activation 221 { 222 XYMode = 0; 223 moveHome(); 224 } 225 else if (reads == CMM) //INVERSE MODE activation 226 { 227 XYMode = 1; 228 moveHome(); 229 230 } 231 232 if (XYMode == 0) 233 directMode(); 234 else if (XYMode == 1) 235 coordinateMode(); 236} 237 238void limits() //Limit angle between angleMax and angle Min 239{ 240 //Base limitation 241 if (baseAngle > baseMax) 242 baseAngle = baseMax; 243 else if (baseAngle < baseMin) 244 baseAngle = baseMin; 245 246 //Servo1 limitation 247 if (servo1Angle > servo1Max) 248 servo1Angle = servo1Max; 249 else if (servo1Angle < servo1Min) 250 servo1Angle = servo1Min; 251 252 //Servo2 limitation 253 if (servo2Angle > servo2Max) 254 servo2Angle = servo2Max; 255 else if (servo2Angle < servo2Min) 256 servo2Angle = servo2Min; 257 258 //Servo3 limitation 259 if (servo3Angle > servo3Max) 260 servo3Angle = servo3Max; 261 else if (servo3Angle < servo3Min) 262 servo3Angle = servo3Min; 263 264 //Servo4 limitation 265 if (servo4Angle > servo4Max) 266 servo4Angle = servo4Max; 267 else if (servo4Angle < servo4Min) 268 servo4Angle = servo4Min; 269 270 //Gripper limitation 271 if (gripperAngle > gripperMax) 272 gripperAngle = gripperMax; 273 else if (gripperAngle < gripperMin) 274 gripperAngle = gripperMin; 275} 276 277///////////////// 278//////MOVES////// 279///////////////// 280 281void moveAll() 282{ 283 limits(); 284 //Base Movement 285 base.write(baseAngle, servoSpeed, false); 286 //Servo1 Movement 287 servo1.write(servo1Angle, servoSpeed, false); 288 //Servo2 Movement 289 servo2.write(servo2Angle, servoSpeed, false); 290 //Servo3 Movement 291 servo3.write(servo3Angle, servoSpeed, false); 292 //Servo4 Movement 293 servo4.write(servo4Angle, servoSpeed, false); 294 //Gripper Movement 295 gripper.write(gripperAngle, servoSpeed, false); 296} 297 298void moveHome() 299{ 300 servoSpeed = servoSpeedSlow; 301 if (XYMode == 0) //Direct Home 302 { 303 servoSpeed = servoSpeedSlow; 304 baseAngle = baseInit; 305 servo1Angle = servo1Init; 306 servo2Angle = servo2Init; 307 servo3Angle = servo3Init; 308 servo4Angle = servo4Init; 309 gripperAngle = gripperInit; 310 } 311 else if (XYMode == 1) //Coordinate Home 312 { 313 x = xInit; 314 y = yInit; 315 gripAngle = gripAngleInit; 316 baseAngle = baseInit; 317 IKcalculation(); 318 } 319 moveAll(); 320} 321 322///////////////// 323//////DKMODE///// 324///////////////// 325 326void directMode() 327{ 328 /*=====Base reading=====*/ 329 if (reads == MB1) //Base Clockwise 330 { 331 baseAngle = baseAngle + 1; 332 delay(timeUp); 333 servoSpeed = servoSpeedFast; 334 } 335 else if (reads == MB2) //Base Counterclockwise 336 { 337 baseAngle = baseAngle - 1; 338 delay(timeUp); 339 servoSpeed = servoSpeedFast; 340 } 341 342 /*=====Servo1 reading=====*/ 343 if (reads == M11) //S1 Clockwise 344 { 345 servo1Angle = servo1Angle + 1; 346 delay(timeUp); 347 servoSpeed = servoSpeedFast; 348 } 349 else if (reads == M12) //S1 Counterclockwise 350 { 351 servo1Angle = servo1Angle - 1; 352 delay(timeUp); 353 servoSpeed = servoSpeedFast; 354 } 355 356 /*=====Servo2 reading=====*/ 357 //Servo2 reading 358 if (reads == M21) //S2 Clockwise 359 { 360 servo2Angle = servo2Angle + 1; 361 delay(timeUp); 362 servoSpeed = servoSpeedFast; 363 } 364 else if (reads == M22) //S2 Counterclockwise 365 { 366 servo2Angle = servo2Angle - 1; 367 delay(timeUp); 368 servoSpeed = servoSpeedFast; 369 } 370 371 /*=====Servo3 reading=====*/ 372 if (reads == M31) //S3 Clockwise 373 { 374 servo3Angle = servo3Angle + 1; 375 delay(timeUp); 376 servoSpeed = servoSpeedFast; 377 } 378 else if (reads == M32) //S3 Counterclockwise 379 { 380 servo3Angle = servo3Angle - 1; 381 delay(timeUp); 382 servoSpeed = servoSpeedFast; 383 } 384 385 /*=====Servo4 reading=====*/ 386 if (reads == M41) //S4 Clockwise 387 { 388 servo4Angle = servo4Angle + 1; 389 delay(timeUp); 390 servoSpeed = servoSpeedFast; 391 } 392 else if (reads == M42) //S4 Counterclockwise 393 { 394 servo4Angle = servo4Angle - 1; 395 delay(timeUp); 396 servoSpeed = servoSpeedFast; 397 } 398 399 /*=====Gripper reading=====*/ 400 if (reads == MG1) //Opening 401 { 402 gripperAngle = 60; 403 delay(timeUp); 404 servoSpeed = servoSpeedFast; 405 } 406 else if (reads == MG2) //Closing 407 { 408 gripperAngle = 0; 409 delay(timeUp); 410 servoSpeed = servoSpeedFast; 411 } 412 413 /*=====Homing reading=====*/ 414 if (reads == MHO) //Set angles to home position 415 { 416 delay(timeUp); 417 moveHome(); 418 } 419 420 /*=====Reading points DIRECT=====*/ 421 if (reads == PRD) //Points reading 422 { 423 savePoint(); 424 } 425 426 /*=====Points sequence DIRECT=====*/ 427 if (reads == MPS) 428 { 429 moveSequence(); 430 } 431} 432 433//////////////////POINTS DIRECT/////////////////////////// 434 435void savePoint() 436{ 437 if (DB == 0) //Save point1 438 { 439 Dpoint1[0] = base.read(); 440 Dpoint1[1] = servo1.read(); 441 Dpoint1[2] = servo2.read(); 442 Dpoint1[3] = servo3.read(); 443 Dpoint1[4] = servo4.read(); 444 Dpoint1[5] = gripper.read(); 445 DB = 1; 446 } 447 else if (DB == 1) //Save point2 448 { 449 Dpoint2[0] = base.read(); 450 Dpoint2[1] = servo1.read(); 451 Dpoint2[2] = servo2.read(); 452 Dpoint2[3] = servo3.read(); 453 Dpoint2[4] = servo4.read(); 454 Dpoint2[5] = gripper.read(); 455 DB = 2; 456 } 457 else if (DB == 2) //Save point3 458 { 459 Dpoint3[0] = base.read(); 460 Dpoint3[1] = servo1.read(); 461 Dpoint3[2] = servo2.read(); 462 Dpoint3[3] = servo3.read(); 463 Dpoint3[4] = servo4.read(); 464 Dpoint3[5] = gripper.read(); 465 DB = 3; 466 } 467 else if (DB == 3) //Save point4 468 { 469 Dpoint4[0] = base.read(); 470 Dpoint4[1] = servo1.read(); 471 Dpoint4[2] = servo2.read(); 472 Dpoint4[3] = servo3.read(); 473 Dpoint4[4] = servo4.read(); 474 Dpoint4[5] = gripper.read(); 475 DB = 4; 476 } 477 else if (DB == 4) //Save point5 478 { 479 Dpoint5[0] = base.read(); 480 Dpoint5[1] = servo1.read(); 481 Dpoint5[2] = servo2.read(); 482 Dpoint5[3] = servo3.read(); 483 Dpoint5[4] = servo4.read(); 484 Dpoint5[5] = gripper.read(); 485 DB = 5; 486 } 487 else if (DB == 5) //Save Point Home 488 { 489 DB = 6; 490 } 491 else if (DB == 6) //Max point allert 492 { 493 delay(1000); 494 } 495 else if (DB == 7) //Reset Points 496 { 497 DB == 0; 498 } 499} 500 501void moveSequence() 502{ 503 servoSpeed = servoSpeedSlow; 504 if (DB == 6 || DB == 7) //5 points sequence + Home 505 { 506 Point1(); 507 moveAll(); 508 delay(timeSeq); 509 Point2(); 510 moveAll(); 511 delay(timeSeq); 512 Point3(); 513 moveAll(); 514 delay(timeSeq); 515 Point4(); 516 moveAll(); 517 delay(timeSeq); 518 Point5(); 519 moveAll(); 520 delay(timeSeq); 521 moveHome(); 522 delay(timeSeq); 523 } 524 else if (DB == 5) //5 points sequence 525 { 526 Point1(); 527 moveAll(); 528 delay(timeSeq); 529 Point2(); 530 moveAll(); 531 delay(timeSeq); 532 Point3(); 533 moveAll(); 534 delay(timeSeq); 535 Point4(); 536 moveAll(); 537 delay(timeSeq); 538 Point5(); 539 moveAll(); 540 delay(timeSeq); 541 } 542 else if (DB == 4) //4 points sequence 543 { 544 Point1(); 545 moveAll(); 546 delay(timeSeq); 547 Point2(); 548 moveAll(); 549 delay(timeSeq); 550 Point3(); 551 moveAll(); 552 delay(timeSeq); 553 Point4(); 554 moveAll(); 555 delay(timeSeq); 556 } 557 else if (DB == 3) //3 points sequence 558 { 559 Point1(); 560 moveAll(); 561 delay(timeSeq); 562 Point2(); 563 moveAll(); 564 delay(timeSeq); 565 Point3(); 566 moveAll(); 567 delay(timeSeq); 568 } 569 else if (DB == 2) //2 points sequence 570 { 571 Point1(); 572 moveAll(); 573 delay(timeSeq); 574 Point2(); 575 moveAll(); 576 delay(timeSeq); 577 } 578 else if (DB == 1) //1 point sequence 579 { 580 Point1(); 581 moveAll(); 582 delay(timeSeq); 583 } 584} 585 586void Point1() 587{ 588 baseAngle = Dpoint1[0]; 589 servo1Angle = Dpoint1[1]; 590 servo2Angle = Dpoint1[2]; 591 servo3Angle = Dpoint1[3]; 592 servo4Angle = Dpoint1[4]; 593 gripperAngle = Dpoint1[5]; 594} 595void Point2() 596{ 597 baseAngle = Dpoint2[0]; 598 servo1Angle = Dpoint2[1]; 599 servo2Angle = Dpoint2[2]; 600 servo3Angle = Dpoint2[3]; 601 servo4Angle = Dpoint2[4]; 602 gripperAngle = Dpoint2[5]; 603} 604void Point3() 605{ 606 baseAngle = Dpoint3[0]; 607 servo1Angle = Dpoint3[1]; 608 servo2Angle = Dpoint3[2]; 609 servo3Angle = Dpoint3[3]; 610 servo4Angle = Dpoint3[4]; 611 gripperAngle = Dpoint3[5]; 612} 613void Point4() 614{ 615 baseAngle = Dpoint4[0]; 616 servo1Angle = Dpoint4[1]; 617 servo2Angle = Dpoint4[2]; 618 servo3Angle = Dpoint4[3]; 619 servo4Angle = Dpoint4[4]; 620 gripperAngle = Dpoint4[5]; 621} 622void Point5() 623{ 624 baseAngle = Dpoint5[0]; 625 servo1Angle = Dpoint5[1]; 626 servo2Angle = Dpoint5[2]; 627 servo3Angle = Dpoint5[3]; 628 servo4Angle = Dpoint5[4]; 629 gripperAngle = Dpoint5[5]; 630} 631 632///////////////// 633//////IKMODE///// 634///////////////// 635 636void coordinateMode() 637{ 638 /*=====X VALUE VARIATION=====*/ 639 if (reads == CX1) //X increase 640 { 641 x = x + 1; 642 delay(timeUp); 643 servoSpeed = servoSpeedFast; 644 } 645 else if (reads == CX2) //X decrease 646 { 647 x = x - 1; 648 delay(timeUp); 649 servoSpeed = servoSpeedFast; 650 } 651 652 /*=====Y VALUE VARIATION=====*/ 653 if (reads == CY1) //Y increase 654 { 655 y = y + 1; 656 delay(timeUp); 657 servoSpeed = servoSpeedFast; 658 } 659 else if (reads == CY2) //Y decrease 660 { 661 y = y - 1; 662 delay(timeUp); 663 servoSpeed = servoSpeedFast; 664 } 665 666 /*=====Z ROTATION VARIATION=====*/ 667 if (reads == CZ1) //Z clockwise rotation 668 { 669 baseAngle = baseAngle + 1; 670 delay(timeUp); 671 servoSpeed = servoSpeedFast; 672 } 673 else if (reads == CZ2) //Z counterclockwise rotation 674 { 675 baseAngle = baseAngle - 1; 676 delay(timeUp); 677 servoSpeed = servoSpeedFast; 678 } 679 680 /*=====GRIP ANGLE VARIATION=====*/ 681 if (reads == CA1) //GripAngle increase 682 { 683 gripAngle = gripAngle + 1; 684 delay(timeUp); 685 servoSpeed = servoSpeedFast; 686 } 687 else if (reads == CA2) //GripAngle decrease 688 { 689 gripAngle = gripAngle - 1; 690 delay(timeUp); 691 servoSpeed = servoSpeedFast; 692 } 693 694 /*=====HOME POSITION IK=====*/ 695 if (reads == MHO) 696 { 697 moveHome(); 698 } 699 700 /*=====GRIPPER READING=====*/ 701 if (reads == MG1) //Opening 702 { 703 gripperAngle = 60; 704 delay(timeUp); 705 servoSpeed = servoSpeedFast; 706 } 707 else if (reads == MG2) //Closing 708 { 709 gripperAngle = 0; 710 delay(timeUp); 711 servoSpeed = servoSpeedFast; 712 } 713 714 /*=====IK READING POINTS=====*/ 715 if (reads == PRD) 716 { 717 savePointIK(); 718 } 719 if (reads == MPS) 720 { 721 moveSequenceIK(); 722 } 723 IKcalculation(); 724} 725 726//////////////////POINTS INVERSE/////////////////////////// 727void savePointIK() 728{ 729 if (DBIK == 0) //Save point1 730 { 731 IKpoint1[0] = x; 732 IKpoint1[1] = y; 733 IKpoint1[2] = gripAngle; 734 IKpoint1[3] = base.read(); 735 DBIK = 1; 736 } 737 else if (DBIK == 1) //Save point2 738 { 739 IKpoint1[0] = x; 740 IKpoint1[1] = y; 741 IKpoint1[2] = gripAngle; 742 IKpoint1[3] = base.read(); 743 DBIK = 2; 744 } 745 else if (DBIK == 2) //Save point3 746 { 747 IKpoint1[0] = x; 748 IKpoint1[1] = y; 749 IKpoint1[2] = gripAngle; 750 IKpoint1[3] = base.read(); 751 DBIK = 3; 752 } 753 else if (DBIK == 3) //Save point4 754 { 755 IKpoint1[0] = x; 756 IKpoint1[1] = y; 757 IKpoint1[2] = gripAngle; 758 IKpoint1[3] = base.read(); 759 DBIK = 4; 760 } 761 else if (DBIK == 4) //Save point5 762 { 763 IKpoint1[0] = x; 764 IKpoint1[1] = y; 765 IKpoint1[2] = gripAngle; 766 IKpoint1[3] = base.read(); 767 DBIK = 5; 768 } 769 else if (DBIK == 5) //Save point Home at last 770 { 771 DBIK = 6; 772 } 773 else if (DBIK == 6) //Max point allert 774 { 775 DBIK = 7; 776 delay(500); 777 } 778 else if (DBIK == 7) //Reset points 779 { 780 DBIK = 0; 781 delay(1000); 782 } 783} 784 785void moveSequenceIK() 786{ 787 servoSpeed = servoSpeedSlow; 788 if (DBIK == 7 || DBIK == 6) //Point 1-2-3-4-5-Home Sequence 789 { 790 IKPoint1(); 791 delay(timeSeq); 792 IKPoint2(); 793 delay(timeSeq); 794 IKPoint3(); 795 delay(timeSeq); 796 IKPoint4(); 797 delay(timeSeq); 798 IKPoint5(); 799 delay(timeSeq); 800 moveHome(); 801 delay(timeSeq); 802 } 803 else if (DBIK == 5) //Point 1-2-3-4-5 Sequence 804 { 805 IKPoint1(); 806 delay(timeSeq); 807 IKPoint2(); 808 delay(timeSeq); 809 IKPoint3(); 810 delay(timeSeq); 811 IKPoint4(); 812 delay(timeSeq); 813 IKPoint5(); 814 delay(timeSeq); 815 } 816 else if (DBIK == 4) //Point 1-2-3-4 Sequence 817 { 818 IKPoint1(); 819 delay(timeSeq); 820 IKPoint2(); 821 delay(timeSeq); 822 IKPoint3(); 823 delay(timeSeq); 824 IKPoint4(); 825 delay(timeSeq); 826 } 827 else if (DBIK == 3) //Point 1-2-3 Sequence 828 { 829 IKPoint1(); 830 delay(timeSeq); 831 IKPoint2(); 832 delay(timeSeq); 833 IKPoint3(); 834 delay(timeSeq); 835 } 836 else if (DBIK == 2) //Point 1-2 Sequence 837 { 838 IKPoint1(); 839 delay(timeSeq); 840 IKPoint2(); 841 delay(timeSeq); 842 } 843 else if (DBIK == 1) //Point 1 Sequence 844 { 845 IKPoint1(); 846 delay(timeSeq); 847 } 848} 849 850int Vx; 851int Vy; 852double pendance; 853int Vz; 854int Vangle; 855double steps; 856double q; 857 858void IKPoint1() 859{ 860 Vx = IKpoint1[0] - x; 861 Vy = IKpoint1[1] - y; 862 Vangle = IKpoint1[2] - gripAngle; 863 Vz = IKpoint1[3] - baseAngle; 864 865 pendance = Vy / Vx; 866 q = y - (pendance * x); 867 868 if (Vx > 0) 869 { 870 for (steps = 0; steps < Vx; steps++) 871 { 872 x++; 873 y = (pendance * x) + q; 874 if (Vz > 0) 875 if (baseAngle < IKpoint1[3]) 876 baseAngle++; 877 if (Vz < 0) 878 if (baseAngle > IKpoint1[3]) 879 baseAngle--; 880 if (Vangle > 0) 881 if (gripAngle < IKpoint1[2]) 882 gripAngle++; 883 if (Vangle < 0) 884 if (gripAngle > IKpoint1[2]) 885 gripAngle--; 886 delay(100); 887 IKcalculation(); 888 moveAll(); 889 } 890 } 891 if (Vx < 0) 892 { 893 for (steps = Vx; steps > 0; steps--) 894 { 895 x--; 896 y = (pendance * x) + q; 897 if (Vz > 0) 898 if (baseAngle < IKpoint1[3]) 899 baseAngle++; 900 if (Vz < 0) 901 if (baseAngle > IKpoint1[3]) 902 baseAngle--; 903 if (Vangle > 0) 904 if (gripAngle < IKpoint1[2]) 905 gripAngle++; 906 if (Vangle < 0) 907 if (gripAngle > IKpoint1[2]) 908 gripAngle--; 909 delay(100); 910 IKcalculation(); 911 moveAll(); 912 } 913 } 914} 915void IKPoint2() 916{ 917 Vx = IKpoint2[0] - x; 918 Vy = IKpoint2[1] - y; 919 Vangle = IKpoint2[2] - gripAngle; 920 Vz = IKpoint2[3] - baseAngle; 921 922 pendance = Vy / Vx; 923 q = y - (pendance * x); 924 925 if (Vx > 0) 926 { 927 for (steps = 0; steps < Vx; steps++) 928 { 929 x++; 930 y = (pendance * x) + q; 931 if (Vz > 0) 932 if (baseAngle < IKpoint2[3]) 933 baseAngle++; 934 if (Vz < 0) 935 if (baseAngle > IKpoint2[3]) 936 baseAngle--; 937 if (Vangle > 0) 938 if (gripAngle < IKpoint2[2]) 939 gripAngle++; 940 if (Vangle < 0) 941 if (gripAngle > IKpoint2[2]) 942 gripAngle--; 943 delay(100); 944 IKcalculation(); 945 moveAll(); 946 } 947 } 948 if (Vx < 0) 949 { 950 for (steps = Vx; steps > 0; steps--) 951 { 952 x--; 953 y = (pendance * x) + q; 954 if (Vz > 0) 955 if (baseAngle < IKpoint2[3]) 956 baseAngle++; 957 if (Vz < 0) 958 if (baseAngle > IKpoint2[3]) 959 baseAngle--; 960 if (Vangle > 0) 961 if (gripAngle < IKpoint2[2]) 962 gripAngle++; 963 if (Vangle < 0) 964 if (gripAngle > IKpoint2[2]) 965 gripAngle--; 966 delay(100); 967 IKcalculation(); 968 moveAll(); 969 } 970 } 971} 972void IKPoint3() 973{ 974 Vx = IKpoint3[0] - x; 975 Vy = IKpoint3[1] - y; 976 Vangle = IKpoint3[2] - gripAngle; 977 Vz = IKpoint3[3] - baseAngle; 978 979 pendance = Vy / Vx; 980 q = y - (pendance * x); 981 982 if (Vx > 0) 983 { 984 for (steps = 0; steps < Vx; steps++) 985 { 986 x++; 987 y = (pendance * x) + q; 988 if (Vz > 0) 989 if (baseAngle < IKpoint3[3]) 990 baseAngle++; 991 if (Vz < 0) 992 if (baseAngle > IKpoint3[3]) 993 baseAngle--; 994 if (Vangle > 0) 995 if (gripAngle < IKpoint3[2]) 996 gripAngle++; 997 if (Vangle < 0) 998 if (gripAngle > IKpoint3[2]) 999 gripAngle--; 1000 delay(100); 1001 IKcalculation(); 1002 moveAll(); 1003 } 1004 } 1005 if (Vx < 0) 1006 { 1007 for (steps = Vx; steps > 0; steps--) 1008 { 1009 x--; 1010 y = (pendance * x) + q; 1011 if (Vz > 0) 1012 if (baseAngle < IKpoint3[3]) 1013 baseAngle++; 1014 if (Vz < 0) 1015 if (baseAngle > IKpoint3[3]) 1016 baseAngle--; 1017 if (Vangle > 0) 1018 if (gripAngle < IKpoint3[2]) 1019 gripAngle++; 1020 if (Vangle < 0) 1021 if (gripAngle > IKpoint3[2]) 1022 gripAngle--; 1023 delay(100); 1024 IKcalculation(); 1025 moveAll(); 1026 } 1027 } 1028} 1029void IKPoint4() 1030{ 1031 Vx = IKpoint4[0] - x; 1032 Vy = IKpoint4[1] - y; 1033 Vangle = IKpoint4[2] - gripAngle; 1034 Vz = IKpoint4[3] - baseAngle; 1035 1036 pendance = Vy / Vx; 1037 q = y - (pendance * x); 1038 1039 if (Vx > 0) 1040 { 1041 for (steps = 0; steps < Vx; steps++) 1042 { 1043 x++; 1044 y = (pendance * x) + q; 1045 if (Vz > 0) 1046 if (baseAngle < IKpoint4[3]) 1047 baseAngle++; 1048 if (Vz < 0) 1049 if (baseAngle > IKpoint4[3]) 1050 baseAngle--; 1051 if (Vangle > 0) 1052 if (gripAngle < IKpoint4[2]) 1053 gripAngle++; 1054 if (Vangle < 0) 1055 if (gripAngle > IKpoint4[2]) 1056 gripAngle--; 1057 delay(100); 1058 IKcalculation(); 1059 moveAll(); 1060 } 1061 } 1062 if (Vx < 0) 1063 { 1064 for (steps = Vx; steps > 0; steps--) 1065 { 1066 x--; 1067 y = (pendance * x) + q; 1068 if (Vz > 0) 1069 if (baseAngle < IKpoint4[3]) 1070 baseAngle++; 1071 if (Vz < 0) 1072 if (baseAngle > IKpoint4[3]) 1073 baseAngle--; 1074 if (Vangle > 0) 1075 if (gripAngle < IKpoint4[2]) 1076 gripAngle++; 1077 if (Vangle < 0) 1078 if (gripAngle > IKpoint4[2]) 1079 gripAngle--; 1080 delay(100); 1081 IKcalculation(); 1082 moveAll(); 1083 } 1084 } 1085} 1086void IKPoint5() 1087{ 1088 Vx = IKpoint5[0] - x; 1089 Vy = IKpoint5[1] - y; 1090 Vangle = IKpoint5[2] - gripAngle; 1091 Vz = IKpoint5[3] - baseAngle; 1092 1093 pendance = Vy / Vx; 1094 q = y - (pendance * x); 1095 1096 if (Vx > 0) 1097 { 1098 for (steps = 0; steps < Vx; steps++) 1099 { 1100 x++; 1101 y = (pendance * x) + q; 1102 if (Vz > 0) 1103 if (baseAngle < IKpoint5[3]) 1104 baseAngle++; 1105 if (Vz < 0) 1106 if (baseAngle > IKpoint5[3]) 1107 baseAngle--; 1108 if (Vangle > 0) 1109 if (gripAngle < IKpoint5[2]) 1110 gripAngle++; 1111 if (Vangle < 0) 1112 if (gripAngle > IKpoint5[2]) 1113 gripAngle--; 1114 delay(100); 1115 IKcalculation(); 1116 moveAll(); 1117 } 1118 } 1119 if (Vx < 0) 1120 { 1121 for (steps = Vx; steps > 0; steps--) 1122 { 1123 x--; 1124 y = (pendance * x) + q; 1125 if (Vz > 0) 1126 if (baseAngle < IKpoint5[3]) 1127 baseAngle++; 1128 if (Vz < 0) 1129 if (baseAngle > IKpoint5[3]) 1130 baseAngle--; 1131 if (Vangle > 0) 1132 if (gripAngle < IKpoint5[2]) 1133 gripAngle++; 1134 if (Vangle < 0) 1135 if (gripAngle > IKpoint5[2]) 1136 gripAngle--; 1137 delay(100); 1138 IKcalculation(); 1139 moveAll(); 1140 } 1141 } 1142} 1143 1144////////////////// 1145//IK CALCULATION// 1146////////////////// 1147 1148void IKcalculation() 1149{ 1150 yR = y - offset; 1151 1152 //1- Translation to G' point 1153 angle2 = 180 - gripAngle; 1154 x2 = x + (link4 * cos(angle2 * (PI / 180.00))); 1155 y2 = yR + (link4 * sin(angle2 * (PI / 180.00))); 1156 1157 //2- Slope calculation 1158 m = y2 / x2; 1159 1160 //3- Circle intersacation 1161 a = 1.00 + sq(m); 1162 b = -(2.00 * x2) - (2.00 * m * y2); 1163 c = sq(x2) + sq(y2) - sq(link3); 1164 xB1 = (-b + sqrt(sq(b) - (4 * a * c))) / (2.00 * a); 1165 xB2 = (-b - sqrt(sq(b) - (4 * a * c))) / (2.00 * a); 1166 xB = min(xB1, xB2); 1167 yB = m * xB; 1168 r = sqrt(sq(xB) + sq(yB)); 1169 1170 //4- Triangle angles 1171 alfa = (acos((sq(link1) + sq(link2) - sq(r)) / (2.00 * link1 * link2))) * (180.00 / PI); 1172 beta = (acos((sq(link2) + sq(r) - sq(link1)) / (2.00 * link2 * r))) * (180.00 / PI); 1173 gamma = (acos((sq(link1) + sq(r) - sq(link2)) / (2.00 * link1 * r))) * (180.00 / PI); 1174 1175 //5- Deltoid angles 1176 delta = (180.00 - beta) * (PI / 180.00); 1177 d = sqrt(sq(link2) + sq(link3) - (2.00 * link2 * link3 * cos(delta))); 1178 alfa2 = 2.00 * ((asin((link3 * sin(delta)) / d)) * (180.00 / PI)); 1179 1180 //6- Joints angles calculation 1181 J1 = gamma + ((atan(y2 / x2)) * (180.00 / PI)); 1182 J2 = alfa - 90.00 + alfa2; 1183 J3 = delta * (180.00 / PI) - 90.00; 1184 sum = J1 + alfa + alfa2 + delta * (180 / PI); 1185 J4 = 270 + angle2 - sum; 1186 1187 //7- Servo angles convertion 1188 servo1Angle = 180 - J1; 1189 servo2Angle = J2; 1190 servo3Angle = 180 - J3; 1191 servo4Angle = J4; 1192} 1193
Joystick Code
arduino
1/* 2 Project: Bracc.ino 3 Date: 09/01/2020 4 Last change: 23/01/2020 5 6 Team: Basso Andrea 7 Delmoro Niccol 8 Gramaglia Giacomo 9 //////JOYPAD CODE////// 10*/ 11//////////////////// 12////DECLARATIONS//// 13//////////////////// 14 15/*==LIBRARIES==*/ 16#include <SoftwareSerial.h> 17SoftwareSerial BTserial(2, 3); // HC-05 = 2-RX(+resistors) | 3-TX 18#include <LiquidCrystal_I2C.h> 19LiquidCrystal_I2C lcd (0x27, 16, 2); //sda=A4 , scl=A5 20 21/*==BUTTONS==*/ 22int button1Pin = 4; //Open Gripper 23int valButton1; 24int button2Pin = 5; //Save Point 25int valButton2; 26int button3Pin = 7; //Close Gripper 27int valButton3; 28int button4Pin = 8; //Move sequence 29int valButton4; 30//Joystick Buttons 31int button5Pin = 9; //Coordinate Mode (NC) 32int valButton5; 33int button6Pin = 6; //Home position 34int valButton6; 35 36/*==JOYSTICK 1==*/ 37int joy1xPin = A0; 38int joy1yPin = A1; 39int valX1; 40int valY1; 41 42/*==JOYSTICK 2==*/ 43int joy2xPin = A2; 44int joy2yPin = A3; 45int valX2; 46int valY2; 47 48 49/*==ANGLE VARIABLES==*/ 50//Base 51int base = 90; 52int baseMax = 180; 53int baseMin = 0; 54//Servo1 55int servo1 = 73; 56int servo1Max = 180; 57int servo1Min = 0; 58//Servo2 59int servo2 = 26; 60int servo2Max = 180; 61int servo2Min = 0; 62//Servo3 63int servo3 = 147; 64int servo3Max = 180; 65int servo3Min = 0; 66//Servo4 67int servo4 = 14; 68int servo4Max = 180; 69int servo4Min = 0; 70 71/*==Movement Commands==*/ 72char MDM = '0'; //Turn on direct motor mode 73char MB1 = 'A'; //Move base Clockwise 74char MB2 = 'B'; //Move base Counterclockwise 75char M11 = 'C'; //Move servo1 Clockwise 76char M12 = 'D'; //Move servo1 Counterclockwise 77char M21 = 'E'; //Move servo2 Clockwise 78char M22 = 'F'; //Move servo2 Counterclockwise 79char M31 = 'G'; //Move servo3 Clockwise 80char M32 = 'H'; //Move servo3 Counterclockwise 81char M41 = 'I'; //Move servo4 Clockwise 82char M42 = 'J'; //Move servo4 Counterclockwise 83char MG1 = 'K'; //Open gripper 84char MG2 = 'L'; //Close gripper 85char MHO = 'M'; //Move home point 86char MPS = 'N'; //Move points sequence 87/*==Point Commands==*/ 88char PRD = 'P'; //Reading point 89/*==Coordinate Commands==*/ 90char CMM = '1'; //Turn in coordinate Mode 91char CX1 = 'R'; //Increase X value 92char CX2 = 'S'; //Decrease X value 93char CY1 = 'T'; //Increase Y value 94char CY2 = 'U'; //Decrease Y value 95char CZ1 = 'V'; //Increase Z angle 96char CZ2 = 'W'; //Decrease Z angle 97char CA1 = 'X'; //Grip angle increase-clockwise 98char CA2 = 'Y'; //Grip angle decrease-counterclockwise 99 100/*==TIME VALUE==*/ 101int Speed = 8; 102 103/*==IK VALUES==*/ 104int XYMode; 105int modeTouch; 106int x = 95; 107int y = 80; 108int angle = 90; 109int DB; 110 111//////////////////// 112////////SETUP/////// 113//////////////////// 114 115void setup() 116{ 117 //Button declarations 118 pinMode(button1Pin, INPUT); 119 pinMode(button2Pin, INPUT); 120 pinMode(button3Pin, INPUT); 121 pinMode(button4Pin, INPUT); 122 pinMode(button5Pin, INPUT); 123 pinMode(button6Pin, INPUT); 124 125 //Joysticks declarations 126 pinMode(joy1xPin, INPUT); 127 pinMode(joy1yPin, INPUT); 128 pinMode(joy2xPin, INPUT); 129 pinMode(joy2yPin, INPUT); 130 131 //Begins 132 Serial.begin(9600); 133 BTserial.begin(38400); 134 lcd.init(); 135 lcd.clear(); 136 lcd.backlight(); 137 lcd.setCursor(0, 0); 138 lcd.print("Inizialitation..."); 139} 140 141//////////////////// 142////////LOOP//////// 143//////////////////// 144 145void loop() 146{ 147 valButton5 = digitalRead(button5Pin); 148 149 //Mode Selection 150 if (valButton5 == LOW && modeTouch == LOW) //Enter in IK Mode 151 { 152 XYMode = 1; 153 modeTouch = 1; 154 delay(Speed); 155 lcd.clear(); 156 BTserial.write(CMM); 157 lcd.setCursor(0, 0); 158 lcd.print("Coordinate XY"); 159 lcd.setCursor(0, 1); 160 lcd.print("Mode..."); 161 //Initialitation 162 x = 95; 163 y = 80; 164 angle = 90; 165 base = 90; 166 delay(1000); 167 } 168 else if (valButton5 == LOW && modeTouch == HIGH) //Exit from IK Mode 169 { 170 XYMode = 0; 171 modeTouch = 0; 172 delay(Speed); 173 lcd.clear(); 174 BTserial.write(MDM); 175 lcd.setCursor(0, 0); 176 lcd.print("Direct motor"); 177 lcd.setCursor(0, 1); 178 lcd.print("Mode..."); 179 //Inizialitation 180 base = 90; 181 servo1 = 73; 182 servo2 = 26; 183 servo3 = 147; 184 servo4 = 14; 185 delay(1000); 186 } 187 188 //Mode actions [IK - DIRECT] 189 if (XYMode == 1) //IK Mode 190 { 191 coordinateMode(); 192 } 193 else if (XYMode == 0) //Direct Mode 194 { 195 readMovement(); 196 } 197 reading(); 198} 199 200//////////////////// 201//////READING/////// 202//////////////////// 203 204void reading() 205{ 206 valButton1 = digitalRead(button1Pin); 207 valButton2 = digitalRead(button2Pin); 208 valButton3 = digitalRead(button3Pin); 209 valButton4 = digitalRead(button4Pin); 210 valButton6 = digitalRead(button6Pin); 211 212 /*===== MG1 - MG2 = GRIPPER =====*/ 213 if (valButton1 == HIGH) //LowButtonLeft - Gripper opening 214 { 215 delay(Speed); 216 lcd.clear(); 217 BTserial.write(MG1); 218 lcd.setCursor(0, 0); 219 lcd.print("Gripper ="); 220 lcd.setCursor(0, 1); 221 lcd.print("Opened"); 222 } 223 else if (valButton3 == HIGH) //LowButtonRight- Gripper Closing 224 { 225 delay(Speed); 226 lcd.clear(); 227 BTserial.write(MG2); 228 lcd.setCursor(0, 0); 229 lcd.print("Gripper ="); 230 lcd.setCursor(0, 1); 231 lcd.print("Closed"); 232 } 233 234 /*===== Homing Button =====*/ 235 if (valButton6 == HIGH) //JoystickRightButton - Active Home 236 { 237 delay(Speed); 238 lcd.clear(); 239 BTserial.write(MHO); 240 lcd.setCursor(0, 0); 241 lcd.print("Homing"); 242 base = 90; 243 servo1 = 73; 244 servo2 = 26; 245 servo3 = 147; 246 servo4 = 14; 247 248 x = 95; 249 y = 80; 250 angle = 90; 251 delay(1500); 252 } 253 254 /*===== Points =====*/ 255 if (valButton2 == HIGH) //HighButtonLeft - Save point position 256 { 257 delay(Speed); 258 lcd.clear(); 259 lcd.setCursor(0, 0); 260 BTserial.write(PRD); 261 savePoint(); 262 delay(1500); 263 } 264 if (valButton4 == HIGH) //HighButtonRight - Move points sequence 265 { 266 delay(Speed); 267 lcd.clear(); 268 BTserial.write(MPS); 269 lcd.setCursor(0, 0); 270 lcd.print("Move points"); 271 lcd.setCursor(0, 1); 272 lcd.print("sequence"); 273 delay(1500); 274 } 275} 276 277void limits() //Limit angle between angleMax and angle Min 278{ 279 //base limitation 280 if (base > baseMax) 281 base = baseMax; 282 else if (base < baseMin) 283 base = baseMin; 284 //Servo1 limitation 285 if (servo1 > servo1Max) 286 servo1 = servo1Max; 287 else if (servo1 < servo1Min) 288 servo1 = servo1Min; 289 //Servo2 limitation 290 if (servo2 > servo2Max) 291 servo2 = servo2Max; 292 else if (servo2 < servo2Min) 293 servo2 = servo2Min; 294 //Servo3 limitation 295 if (servo3 > servo3Max) 296 servo3 = servo3Max; 297 else if (servo3 < servo3Min) 298 servo3 = servo3Min; 299 //Servo4 limitation 300 if (servo4 > servo4Max) 301 servo4 = servo4Max; 302 else if (servo4 < servo4Min) 303 servo4 = servo4Min; 304} 305 306//////////////////// 307//////DIRECT//////// 308//////////////////// 309 310void readMovement() 311{ 312 valX1 = analogRead(joy1xPin); 313 valY1 = analogRead(joy1yPin); 314 valX2 = analogRead(joy2xPin); 315 valY2 = analogRead(joy2yPin); 316 317 /*===== MB1 - MB2 = BASE =====*/ 318 if (valX1 > 700 && valX2 > 700) //joy1-2 SenseLeft - Base Clockwise 319 { 320 delay(Speed); 321 lcd.clear(); 322 BTserial.write(MB1); 323 base = base + 1; 324 lcd.setCursor(0, 0); 325 lcd.print("Base Angle="); 326 lcd.setCursor(0, 1); 327 lcd.print(base); 328 } 329 else if (valX1 < 350 && valX2 < 350) //joy1-2 SenseRight - Base Counterclockwise 330 { 331 delay(Speed); 332 lcd.clear(); 333 BTserial.write(MB2); 334 base = base - 1; 335 lcd.setCursor(0, 0); 336 lcd.print("Base Angle="); 337 lcd.setCursor(0, 1); 338 lcd.print(base); 339 } 340 341 /*===== M11 - M12 = SERVO 1 =====*/ 342 if (valY1 > 700 && valY2 > 700) //Joy1-2 SenseDown - Servo1 Clockwise 343 { 344 delay(Speed); 345 lcd.clear(); 346 BTserial.write(M11); 347 servo1 = servo1 + 1; 348 lcd.setCursor(0, 0); 349 lcd.print("Servo1 Angle ="); 350 lcd.setCursor(0, 1); 351 lcd.print(servo1); 352 } 353 else if (valY1 < 350 && valY2 < 350) //Joy1-2 SenseUp - Servo1 Counterclockwise 354 { 355 delay(Speed); 356 lcd.clear(); 357 BTserial.write(M12); 358 servo1 = servo1 - 1; 359 lcd.setCursor(0, 0); 360 lcd.print("Servo1 Angle ="); 361 lcd.setCursor(0, 1); 362 lcd.print(servo1); 363 } 364 365 /*===== M21 - M22 = SERVO 2 =====*/ 366 if (valY1 < 350 && valY2 > 400 && valY2 < 600) //Joy1 SenseDown - Servo2 Clockwise 367 { 368 delay(Speed); 369 lcd.clear(); 370 BTserial.write(M21); 371 servo2 = servo2 + 1; 372 lcd.setCursor(0, 0); 373 lcd.print("Servo2 Angle ="); 374 lcd.setCursor(0, 1); 375 lcd.print(servo2); 376 } 377 else if ( valY1 > 700 && valY2 < 600 && valY2 > 400) //Joy1 SenseUp - Servo2 Counterclockwise 378 { 379 delay(Speed); 380 lcd.clear(); 381 BTserial.write(M22); 382 servo2 = servo2 - 1; 383 lcd.setCursor(0, 0); 384 lcd.print("Servo2 Angle ="); 385 lcd.setCursor(0, 1); 386 lcd.print(servo2); 387 } 388 389 /*===== M31 - M32 = SERVO 3 =====*/ 390 else if (valY2 > 700 && valY1 > 400 && valY1 < 600) //Joy2 SenseDown - Servo3 Clockwise 391 { 392 delay(Speed); 393 lcd.clear(); 394 BTserial.write(M31); 395 servo3 = servo3 + 1; 396 lcd.setCursor(0, 0); 397 lcd.print("Servo3 Angle ="); 398 lcd.setCursor(0, 1); 399 lcd.print(servo3); 400 } 401 else if (valY2 < 350 && valY1 > 400 && valY1 < 600) //Joy2 SenseUp - Servo3 Counterclockwise 402 { 403 delay(Speed); 404 lcd.clear(); 405 BTserial.write(M32); 406 servo3 = servo3 - 1; 407 lcd.setCursor(0, 0); 408 lcd.print("Servo3 Angle ="); 409 lcd.setCursor(0, 1); 410 lcd.print(servo3); 411 } 412 413 /*===== M41 - M42 = SERVO 4 =====*/ 414 if (valX1 > 700 && valX2 < 350) //Joy1 SenseLeft 2 SenseRight - Servo4 Clockwise 415 { 416 delay(Speed); 417 lcd.clear(); 418 BTserial.write(M41); 419 servo4 = servo4 + 1; 420 lcd.setCursor(0, 0); 421 lcd.print("Servo4 Angle ="); 422 lcd.setCursor(0, 1); 423 lcd.print(servo4); 424 } 425 else if (valX1 < 350 && valX2 > 700) //Joy1 SenseRight 2 SenseLeft - Servo4 Counterclockwise 426 { 427 delay(Speed); 428 lcd.clear(); 429 BTserial.write(M42); 430 servo4 = servo4 - 1; 431 lcd.setCursor(0, 0); 432 lcd.print("Servo4 Angle ="); 433 lcd.setCursor(0, 1); 434 lcd.print(servo4); 435 } 436 limits(); 437} 438 439//////////////////// 440//////INVERSE/////// 441//////////////////// 442 443void coordinateMode() 444{ 445 valX1 = analogRead(joy1xPin); 446 valY1 = analogRead(joy1yPin); 447 valX2 = analogRead(joy2xPin); 448 valY2 = analogRead(joy2yPin); 449 450 /*==X VARIATION - JOYSTICK1 RIGHT/LEFT==*/ 451 if (valX1 > 700) //X Increase - right 452 { 453 delay(Speed); 454 BTserial.write(CX1); 455 x = x + 1; 456 lcd.clear(); 457 } 458 else if (valX1 < 350) //X Decrease - left 459 { 460 delay(Speed); 461 BTserial.write(CX2); 462 x = x - 1; 463 lcd.clear(); 464 } 465 466 /*==Y VARIATION - JOYSTICK1 UP/DOWN==*/ 467 if (valY1 > 700) //Y increase - Up 468 { 469 delay(Speed); 470 BTserial.write(CY1); 471 y = y + 1; 472 lcd.clear(); 473 } 474 else if (valY1 < 350) //Y Decrease - Down 475 { 476 delay(Speed); 477 BTserial.write(CY2); 478 y = y - 1; 479 lcd.clear(); 480 } 481 482 /*==Z BASE ROTATION==*/ 483 if (valX2 > 700) //Base rotation clockwise 484 { 485 delay(Speed); 486 BTserial.write(CZ1); 487 base = base + 1; 488 lcd.clear(); 489 } 490 else if (valX2 < 350) //Base rotation counterclockwise 491 { 492 delay(Speed); 493 BTserial.write(CZ2); 494 base = base - 1; 495 lcd.clear(); 496 } 497 498 /*==GRIP ANGLE VARIATION==*/ 499 if (valY2 > 700) //Grip clockwise (+) 500 { 501 delay(Speed); 502 BTserial.write(CA1); 503 angle = angle - 1; 504 lcd.clear(); 505 } 506 else if (valY2 < 350) //Grip counterclockwise (-) 507 { 508 delay(Speed); 509 BTserial.write(CA2); 510 angle = angle - 1; 511 lcd.clear(); 512 } 513 514 lcd.setCursor(0, 0); 515 lcd.print("X"); 516 lcd.setCursor(1, 0); 517 lcd.print(x); 518 lcd.setCursor(6, 0); 519 lcd.print("Y"); 520 lcd.setCursor(7, 0); 521 lcd.print(y); 522 lcd.setCursor(12, 0); 523 lcd.print("Z="); 524 lcd.setCursor(13, 0); 525 lcd.print(base); 526 lcd.setCursor(0, 1); 527 lcd.print("GripAngle="); 528 lcd.setCursor(10, 1); 529 lcd.print(angle); 530} 531 532//////////////////// 533////POINT SAVE////// 534//////////////////// 535 536void savePoint() 537{ 538 if (DB == 0) //Save point1 539 { 540 lcd.print("Saved point 1"); 541 DB = 1; 542 } 543 else if (DB == 1) //Save point2 544 { 545 lcd.print("Saved point 2"); 546 DB = 2; 547 } 548 else if (DB == 2) //Save point3 549 { 550 lcd.print("Saved point 3"); 551 DB = 3; 552 } 553 else if (DB == 3) //Save point4 554 { 555 lcd.print("Saved point 4"); 556 DB = 4; 557 } 558 else if (DB == 4) //Save point5 559 { 560 lcd.print("Saved point 5"); 561 DB = 5; 562 } 563 else if (DB == 5) //Save point Home 564 { 565 lcd.print("Last Point"); 566 lcd.setCursor(0, 1); 567 lcd.print("= Home"); 568 DB = 6; 569 } 570 else if (DB == 6) //Max point allarm 571 { 572 lcd.print("MAX Points!!"); 573 lcd.setCursor(0, 1); 574 lcd.print("click for reset..."); 575 DB = 7; 576 } 577 else if (DB == 7) //Reset poits memories 578 { 579 lcd.print("Reset points"); 580 lcd.setCursor(0, 1); 581 lcd.print("Memories"); 582 DB = 0; 583 } 584} 585
Downloadable files
Arm's Circuits
Arm's Circuits
Joystick's circuits
Joystick's circuits
Joystick's circuits
Joystick's circuits
Arm's Circuits
Arm's Circuits
Documentation
Ground_LaserCut
Ground_LaserCut
Plexiglass_BaseFlat_LaserCut
Plexiglass_BaseFlat_LaserCut
Plexiglass_BaseFlat_LaserCut
Plexiglass_BaseFlat_LaserCut
ARMStructure_LaserCut
ARMStructure_LaserCut
Ground_LaserCut
Ground_LaserCut
Comments
Only logged in users can leave comments
BassoAndrea02
0 Followers
•0 Projects
Table of contents
Intro
21
0