Bracc.ino - Robotic Arm

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

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