Robotic Arm Plays Connect-Four

Play Connect Four (the common board game) against a robotic arm.

Mar 7, 2020

•

5600 views

•

8 respects

arm

6dof

robotic arm

Components and supplies

MG966R

RGB LCD Shield Kit, 16x2 Character Display

Remote Controller IR 38 khz and receiver HX1838

Robotic Arm 6dof model ROT3U

Jumper wires (generic)

PCA9685 16 channel 12 Bit, Module I2C Driver Of servo motors

Arduino Due

Project description

Code

Robotic Arm Code

arduino

This is the arduino Code

1
2#include <Wire.h>
3#include <Adafruit_PWMServoDriver.h>
4#include  <IRremote2.h>
5#include <LiquidCrystal.h>
6int StartPos [3] = {0,-290,240};
7int  FourPos [6][7][3] = {  { { 95,150, 65},{ 60,145, 75},{ 28,135, 70},{  0,130, 70},{  -25,130, 70},{ -58,125, 75},{ -90,120, 65} },                   //x,y,z
8                           {  { 95,185, 70},{ 65,185, 70},{ 35,185, 70},{  0,180, 70},{ -30,175, 70},{ -63,170,  65},{ -95,165, 65} },
9                           { { 95,210, 75},{ 65,220, 75},{  35,220, 75},{  0,208, 70},{ -30,213, 75},{ -65,208, 70},{-100,203, 70} },
10                           {  { 95,250, 80},{ 65,250, 80},{ 35,245, 80},{  0,245, 80},{ -28,245, 80},{ -63,245,  80},{ -97,235, 75} },
11                           { { 95,290, 90},{ 65,285, 90},{  35,285, 90},{  5,280, 90},{ -30,285, 90},{ -63,280, 90},{ -98,280, 90} },
12                           {  {103,325,100},{ 68,325,100},{ 35,325,100},{  5,320,100},{ -25,325,100},{ -63,323,100},{  -98,318, 90} }, };
13#define SKILL 10
14#define ROW 6
15#define COLUMN 7
16  
17int board[ROW*COLUMN];
18long int depth,skill;
19unsigned long int nodes;
20/*  ---------- IR VARIABLE ---------- */
21// RECV_PIN = 52;
22IRrecv irrecv(52);
23decode_results  results;
24
25/* ---------- LIQUIDCRYSTAL VARIABLE ---------- */
26// pin lcd:  rs = 8, en = 9, d4 = 4, d5 = 5, d6 = 6, d7 = 7;
27LiquidCrystal lcd(8, 9, 4, 5,  6, 7);
28
29/* ---------- ROBOTIC ARM VARIABLE ---------- */
30Adafruit_PWMServoDriver  pwm = Adafruit_PWMServoDriver();
31#define BASE_HGT 115 //base height
32/*#define  HUMERUS  105   shoulder-to-elbow "bone"
33  #define ULNA     100   elbow-to-wrist    "bone"
34  #define GRIPPER  170   wrist-to-gripper  "bone"   */
35#define  SERVO1  1
36#define SERVO2  2
37#define SERVO3  3
38#define SERVO4  4
39#define  SERVO5  5
40#define SERVO6  6
41
42#define SERVOMIN1  140
43#define SERVOMAX1  475
44#define SERVOMIN2  120
45#define SERVOMAX2  415
46#define SERVOMIN3  170
47#define  SERVOMAX3  465
48#define SERVOMIN4  140
49#define SERVOMAX4  485
50#define SERVOMIN5  105
51#define SERVOMAX5  415
52#define SERVOMIN6  220
53#define SERVOMAX6  355
54unsigned  int ServoMinMax[6][2] = {{SERVOMIN1,SERVOMAX1},
55                                  {SERVOMIN2,SERVOMAX2},
56                                  {SERVOMIN3,SERVOMAX3},
57                                  {SERVOMIN4,SERVOMAX4},
58                                  {SERVOMIN5,SERVOMAX5},
59                                  {SERVOMIN6,SERVOMAX6}};
60
61float  x1,x2,x3,x4,x5,x6 = 90;                          // servo position
62float oldx1,oldx2,oldx3,oldx4,oldx5,oldx6  = x1;        // old servo position
63
64float xaxis =   0;
65float yaxis = 150;
66float  zaxis = 100;
67
68bool checkmin = true;
69bool checkmax = true;
70
71/* ----------  setup ---------- */
72void setup() {
73  Serial.begin(115200);
74  randomSeed(analogRead(A0));
75  lcd.begin(16, 2);
76  lcd.clear();
77  lcd.print(">> 4 in a row <<");
78  lcd.setCursor(0,1);
79  lcd.print("   by Danny003");
80  delay(2000);
81  pwm.begin();
82  pwm.setPWMFreq(50);
83  MyServoWrite(SERVO2,90);
84  delay(1000);
85  Set_Arm(xaxis,yaxis,zaxis);
86  irrecv.enableIRIn();
87  lcd.clear();
88  delay(1000);
89  ShowWin();
90  delay(1000);
91  ShowDefeat();
92  delay(1000);
93  Set_Arm(xaxis,yaxis,zaxis);
94  lcd.print("Vuoi calibrare ? ");
95  lcd.setCursor(0,1);
96  lcd.print(" 1  = SI   0 = No");
97  if ( Read_RC() == 1 ) { CalibraPos() ; }
98}
99
100void  loop() {
101  int move,j,i,coins;
102  coins = ROW*COLUMN;
103  skill = SKILL-5;
104  int choice = 0;
105  for (i=0; i<(ROW*COLUMN); i++) {board[i] = 0;}
106  lcd.clear();
107  lcd.print("Choose Who Start");
108  lcd.setCursor(0,1);
109  lcd.print("CH+=Comp  CH-=You");
110  Serial.println("Choose Who Start");
111  Serial.println("CH+=Comp  CH-=You");
112  choice = Read_RC();   
113  if (choice==10) { 
114    display_board();
115    while(coins!=0)  {
116      if (coins==34) {skill=SKILL-4;} /* quick start */
117      if (coins==30) {skill=SKILL-3;} /* quick start */
118      if (coins==26)  {skill=SKILL-2;} /* quick start */
119      if (coins==22) {skill=SKILL-1;} /* quick  start */
120      if (coins==18) {skill=SKILL;  } /* to  maximum */
121      do  {     
122        lcd.clear();
123        lcd.print(" choose column  ");
124        lcd.setCursor(0,1);
125        lcd.print("   from 1 to 7  ");
126        Serial.println("choose column  [1-7]...");
127        move=Read_RC();
128        if (board[move-1]!=0) {
129          lcd.clear();
130          lcd.print("column is full");
131          Serial.println("column is  full");
132          delay(500); }
133        if ((move<1)||(move>7)) {
134          lcd.clear();
135          lcd.print("column not valid"); 
136          Serial.println("column  is not valid");
137          delay(500); }
138      } while((board[move-1]!=0)||(move<1)||(move>7));
139      lcd.clear();
140      lcd.print("You moved in ");
141      lcd.setCursor(13,0);
142      lcd.print(move);
143      lcd.setCursor(0,1);
144      lcd.print("I am thinking...");  
145      move--;
146      j = ROW-1;
147      while ((board[move+j*COLUMN]!=0)&&(j>=0))  {j--;}
148      board[move+j*COLUMN] = 1;
149      coins--;
150      display_board();
151      if(checkwin(1,move,4)) {
152        lcd.clear();
153        lcd.print("    Player  ");
154        lcd.setCursor(0,1);
155        lcd.print("    W I N S   ");
156        Serial.println("You win\
157");
158        ShowDefeat();
159        delay(3000);
160        break;
161      }
162      move = best_move(-1);
163      j = ROW-1;
164      while ((board[move+j*COLUMN]!=0)&&(j>=0)) {j--;}
165      board[move+j*COLUMN]  = -1;
166      Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),FourPos[j][move][0],FourPos[j][move][1],FourPos[j][move][2],90+degrees(atan2(-FourPos[j][move][0],abs(FourPos[j][move][1]))));
167      display_board();
168      lcd.clear();
169      lcd.print(" CPU move in ");
170      lcd.print(move+1);
171      Serial.println("CPU move in ");
172      Serial.print(move+1);
173      coins--;
174      if (checkwin(-1,move,4)) {
175        display_board();
176        lcd.clear();
177        lcd.print("    Computer  ");
178        lcd.setCursor(0,1);
179        lcd.print("    W I N S   ");
180        Serial.println("CPU wins\
181");
182        ShowWin();
183        delay(3000);
184        break;
185      }
186      Serial.println("");
187    }
188    Serial.println("");
189  }
190  else if (choice == 11) {
191    display_board();
192    while(coins!=0) {
193      if (coins==34) skill=SKILL-4; /* quick start */
194      if (coins==30) skill=SKILL-3; /* quick start */
195      if (coins==26) skill=SKILL-2;  /* quick start */
196      if (coins==22) skill=SKILL-1; /* quick start */
197      if  (coins==18) skill=SKILL;   /* ...to maximum */
198      if (coins==ROW*COLUMN) {move=(random(0,8));}
199      else  { move = best_move(-1); }
200      j = ROW-1;
201      while ((board[move+j*COLUMN]!=0)&&(j>=0))  {j--;}
202      board[move+j*COLUMN] = -1;
203      display_board();
204      Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),FourPos[j][move][0],FourPos[j][move][1],FourPos[j][move][2],90+degrees(atan2(-FourPos[j][move][0],abs(FourPos[j][move][1]))));
205      lcd.clear();
206      lcd.print(" CPU move in ");
207      lcd.print(move+1);
208      Serial.println("CPU move in ");
209      Serial.print(move+1);
210      coins--;
211      if (checkwin(-1,move,4)) {
212        display_board();
213        lcd.clear();
214        lcd.print("    Computer  ");
215        lcd.setCursor(0,1);
216        lcd.print("    W I N S   ");
217        Serial.println("CPU wins\
218");
219        ShowWin();
220        delay(3000);
221        break;
222      }
223      Serial.println("");
224      do {     
225        lcd.clear();
226        lcd.print(" choose column  ");
227        lcd.setCursor(0,1);
228        lcd.print("   from 1 to 7  ");
229        Serial.println("choose  column [1-7]...");
230        move=Read_RC();
231        if (board[move-1]!=0)  {
232          lcd.clear();
233          lcd.print("column is full");
234          Serial.println("column  is full");
235          delay(500);}
236        if ((move<1)||(move>7)) {
237          lcd.clear();
238          lcd.print("column not valid"); 
239          Serial.println("column  is not valid");
240          delay(500);}
241      } while ((board[move-1]!=0)||(move<1)||(move>7));
242      lcd.clear();
243      lcd.print("You moved in ");
244      lcd.setCursor(13,0);
245      lcd.print(move);
246      lcd.setCursor(0,1);
247      lcd.print("I am thinking...");  
248      move--;
249      j = ROW-1;
250      while ((board[move+j*COLUMN]!=0)&&(j>=0))  {j--;}
251      board[move+j*COLUMN] = 1;
252      coins--;
253      display_board();
254      if (checkwin(1,move,4)) {
255        Serial.println("You win\
256");
257        lcd.clear();
258        lcd.print("    Player  ");
259        lcd.setCursor(0,1);
260        lcd.print("    W I N S   ");
261        ShowDefeat();
262        delay(3000);
263        break;  
264      }
265    
266      Serial.println("");
267    }  
268  }
269}
270
271/*  ---------- MyServoWrite ---------- */
272/* This funciont is used to move a singular  servo*/
273void MyServoWrite(int servo, float gradi) {
274     pwm.setPWM( servo,  0, MapNew(gradi, 0.0, 180.0, ServoMinMax[servo-1][0], ServoMinMax[servo-1][1] ));
275}
276
277/*  ---------- MyServoWriteAll ---------- */
278/* This funciont is used to move the  first 4 servo, it has been created to avoid to repeat the funciont MyServoWrite  4 times */
279void MyServoWriteAll(float y1,float y2,float y3,float y4) {
280  pwm.setPWM(  SERVO1, 0, MapNew(y1, 0.0, 180.0, ServoMinMax[SERVO1-1][0], ServoMinMax[SERVO1-1][1]  ));
281  pwm.setPWM( SERVO2, 0, MapNew(y2, 0.0, 180.0, ServoMinMax[SERVO2-1][0],  ServoMinMax[SERVO2-1][1] ));
282  pwm.setPWM( SERVO3, 0, MapNew(y3, 0.0, 180.0,  ServoMinMax[SERVO3-1][0], ServoMinMax[SERVO3-1][1] ));
283  pwm.setPWM( SERVO4,  0, MapNew(y4, 0.0, 180.0, ServoMinMax[SERVO4-1][0], ServoMinMax[SERVO4-1][1] ));
284}
285
286/*  ---------- MyServoWriteGradual ---------- */
287/*This function is similar to MyServoWriteAll;  but to avoid too rapid movement, 
288 in particular when the robotic arm goes from  a position to another one,this function creates 
289 a smooth and simultaneous movement  of the 4 servo, whose time depends on the variation of the bigger angle*/
290void  MyServoWriteGradual( float newpos1, float newpos2, float newpos3, float newpos4  ) {
291  float oldpos1 = oldx1;
292  float oldpos2 = oldx2;
293  float oldpos3 =  oldx3;
294  float oldpos4 = oldx4;
295  float differ1 = newpos1 - oldpos1;
296  float  differ2 = newpos2 - oldpos2;
297  float differ3 = newpos3 - oldpos3;
298  float  differ4 = newpos4 - oldpos4;
299  float largest = max(abs(differ1),max(abs(differ2),  max(abs(differ3),abs(differ4))));
300  for ( int i = 0; i <= largest; i++ ) {
301     oldpos1 += (differ1/largest);
302     oldpos2 += (differ2/largest);
303     oldpos3  += (differ3/largest);
304     oldpos4 += (differ4/largest);
305     MyServoWriteAll(oldpos1,oldpos2,oldpos3,oldpos4);
306     delay(10);       
307  } 
308  MyServoWriteAll(newpos1,newpos2,newpos3,newpos4);
309}  
310 
311/* ---------- Set_Arm ---------- */
312/* This function is the most important  for the movement of the robotic arm, 
313   because it calculates the postions of  the first 4 servo from the three coordinates x,y,z(taken from TakePos and TrisPos).
314   The following code is specific to the configuration of MY robotic arm, so if  you want to use it, you have to adapt it to your arm.*/
315void Set_Arm( float x,  float y, float z) {
316  oldx1 = x1;
317  oldx2 = x2;
318  oldx3 = x3;
319  oldx4  = x4;
320  z = z-BASE_HGT;
321  float dist_y_z = sqrt( sq(z) +  sq(x) + sq(y) );  
322  if ( dist_y_z < 120.0 ) { checkmin=false; } 
323    else { checkmin=true;  }
324  if ( dist_y_z > 370.0 ) { checkmax=false; } 
325    else { checkmax=true;  }
326  if ( checkmin && checkmax ) {
327    float alfa = 180.0-(degrees(acos((-27500  + sqrt( sq(27500) - 71400*(14225 - sq(dist_y_z))))/71400)));
328    float gamma  = degrees(acos((27875 -sq(dist_y_z) - (34000*cos(radians(alfa))))/(-210*dist_y_z)));
329    float omega = degrees(acos(sqrt(sq(x)+sq(y))/dist_y_z));
330    x1 = 90.0-degrees(atan2(-x,abs(y)));
331    if ( z > 0.0 ) { x2=180.0-(gamma+omega); }
332      else { x2=180.0-(gamma-omega);  }  
333    if ( y < 0.0) { x3=x4=(alfa-90.0); }
334      else {
335        x3=x4=(270.0-alfa);
336        x2=180.0-x2;
337        x1=180.0-x1;
338      }
339    if ((abs(oldx1-x1)+abs(oldx2-x2)+abs(oldx3-x3)+abs(oldx4-x4))  > 10.0) { MyServoWriteGradual(x1,x2,x3,x4); }
340      else { MyServoWriteAll(x1,x2,x3,x4);  }
341  } 
342}
343
344
345/* ---------- Reach_Position ---------- */
346/*Through  this function, the Robotic Arm grabs a pawn  from a position and it leaves the pawn  in another position  */
347void Reach_Position ( float fromx, float fromy, float  fromz, float fromangle, float tox, float toy, float toz, float toangle ) {
348  Serial.println("test");
349  Serial.println(tox);
350  Serial.println(toy);
351  Serial.println(toz);
352  Serial.println(toangle);
353  Set_Arm( fromx, fromy+50, fromz+50.0 );
354  delay(300);
355  MyServoWrite( SERVO6,  0 );
356  MyServoWrite( SERVO5, fromangle );
357  delay(1000);
358  Set_Arm( fromx,  fromy+50, fromz );
359  delay(300);
360  Set_Arm( fromx, fromy, fromz );
361  delay(300);
362  MyServoWrite( SERVO6, 165 );
363  delay(500);
364  Set_Arm( fromx, fromy+100,  fromz );
365  delay(500);
366  Set_Arm( tox, toy, toz+150.0);
367  delay(500);
368  MyServoWrite( SERVO5, toangle );
369  delay(300);
370  Set_Arm( tox, toy, toz+50.0);
371  delay(500);
372  Set_Arm( tox, toy, toz );
373  delay(300);
374  MyServoWrite(  SERVO6, 10 );
375  delay(500);
376  Set_Arm( tox, toy, toz + 50.0);
377  delay(500);
378  Set_Arm( xaxis = 0.0, yaxis = 130.0 , zaxis = 200.0 );
379  MyServoWrite( SERVO5,  90 );
380  }
381
382/* ---------- MapNew ---------- */
383/*This function has been  created because the Arduino map() has not the funciont  constrain() and it does  not return a int value */
384int MapNew( float x, float in_min, float in_max, float  out_min, float out_max ) {
385   return round((constrain(x,in_min,in_max) - in_min)  * (out_max - out_min) / (in_max - in_min) + out_min);
386}
387 
388/* ----------  Check_Win ---------- */
389/*This function check all the 8 possible combinations  to win at tris. */
390
391int checkwin(int player,int column,int lenght) {   
392  long int j,r,l,i,height;
393    lenght--;
394    i = column;
395    j = ROW-1;
396    while(board[i+j*COLUMN]!=0) j--;
397    j++;
398    height = j;
399 
400    r  = 0;
401    l = 0;
402    while(((++i)<COLUMN)&&(board[i+j*COLUMN]==player)) r++;
403    i = column;
404    while(((--i)>=0)&&(board[i+j*COLUMN]==player)) l++;
405    if  ((r+l)>=lenght) return 1;
406    i = column;
407 
408    r = 0;
409    if((j<0)){r--;}
410    while(((++j)<ROW)&&(board[i+j*COLUMN]==player)) r++;
411    if (r>=lenght) return  1;
412    j = height;
413 
414    r = 0;
415    l = 0;
416    while(((++i)<COLUMN)&&((++j)<ROW)&&(board[i+j*COLUMN]==player))  r++;
417    i = column;
418    j = height;
419    while(((--i)>=0)&&((--j)>=0)&&(board[i+j*COLUMN]==player))  l++;
420    if ((r+l)>=lenght) return 1;
421    i = column;
422    j = height;
423  
424    r = 0;
425    l = 0;
426    while(((++i)<COLUMN)&&((--j)>=0)&&(board[i+j*COLUMN]==player))  r++;
427    i = column;
428    j = height;
429    while(((--i)>=0)&&((++j)<ROW)&&(board[i+j*COLUMN]==player))  l++;
430    if ((r+l)>=lenght) return 1;
431 
432    return 0;
433}
434
435
436int  extimated_value(int player) {
437    long int i,j,value,l; 
438    value = 0;
439    for(l=2;l<4;l++)
440    {
441        for(i=0;i<COLUMN;i++)
442        {
443            if(checkwin(player,i,l)) value = value + l;
444        }
445    }
446  
447    return value;
448}
449
450int goodness(int player,int depth,int column,int  trigger) {
451  long int max,i,value,j;
452    max = -200;
453    if (checkwin(-player,column,4))  return -128;
454    if (depth==0) return 0;
455    for(i=0;i<COLUMN;i++)
456    {
457        if(board[i]==0)
458        {
459            j = ROW-1;
460            while(board[i+j*COLUMN]!=0)  j--;
461            board[i+j*COLUMN] = player;
462            nodes++;
463            value  = -goodness(-player,depth-1,i,-max)/2;
464            board[i+j*COLUMN] = 0;
465            if (value>max) max = value;
466            if (value>trigger) return  max;
467        }
468    }
469    return max;
470}
471
472int best_move(int player)
473{
474    long int i,j,max,value,best,same,trigger,old,att;
475    max = -100;
476    best  = -1;
477    long int maxnodes = -1;
478    for(i=0;i<COLUMN;i++)
479    {
480        if(board[i]==0)
481        {
482            nodes = 0;
483            j = ROW-1;
484            while((board[i+j*COLUMN]!=0)&&(j>=0))  j--;
485            board[i+j*COLUMN] = player;
486            value = -goodness(-player,skill,i,200);
487            //printf("\
488move %d goodness: %d   tree size for this move: %d nodes",i+1,value,nodes);
489            board[i+j*COLUMN] = 0;
490            if ((value == max) && (nodes>maxnodes))  {
491          maxnodes=nodes;
492        max = value;
493                best  = i;}
494      if (value>max) {
495          maxnodes=nodes;
496        max = value;
497                best = i; } 
498
499         
500      }
501    }
502    if(best==-1)
503    {
504        for(i=0;i<COLUMN;i++) if(board[i]==0) return i;
505    }
506 
507    return best;
508}
509
510
511
512/* ---------- Read_RC ---------- */
513/* This  function read the button you pressed  */
514int Read_RC(){
515  int returnvalue=99;
516  while (returnvalue==99){
517    while (!irrecv.decode(&results)) {};
518    switch(results.value){
519      case 0xFFA25D: returnvalue = 10; break;     // CH-
520      case 0xFFE21D:  returnvalue = 11; break;     // CH+
521      case 0xFF6897: returnvalue =  0; break;     //  0
522      case 0xFF30CF: returnvalue =  1; break;     //  1 
523      case  0xFF18E7: returnvalue =  2; break;     //  2
524      case 0xFF7A85: returnvalue  =  3; break;     //  3
525      case 0xFF10EF: returnvalue =  4; break;     //  4
526      case 0xFF38C7: returnvalue =  5; break;     //  5
527      case 0xFF5AA5:  returnvalue =  6; break;     //  6
528      case 0xFF42BD: returnvalue =  7; break;     //  7
529      case 0xFF4AB5: returnvalue =  8; break;     //  8
530      case  0xFF52AD: returnvalue =  9; break;     //  9     
531    }
532    irrecv.resume();      
533  }
534  return returnvalue;
535}
536
537
538
539void display_board(void)  {
540  long int i,j;
541  Serial.println("");
542  for(j=0; j<ROW; j++) {
543    for(i=0; i<COLUMN; i++) {
544    if (board[i+j*COLUMN]== 1) {Serial.print("X  ");}
545    if (board[i+j*COLUMN]==-1) {Serial.print("O ");}
546    if (board[i+j*COLUMN]==  0) {Serial.print(". ");}
547    }
548    Serial.println("");
549  }
550  for(i=0;  i<(COLUMN*2)-1; i++) {Serial.print("-");}
551  Serial.println("");
552  for(i=0;  i<COLUMN; i++) {Serial.print(i+1),Serial.print(" ");}
553  Serial.println("");
554}
555
556void  CalibraPos() {
557  int scelta = 99;
558  int col,row;
559  while( scelta != 0 )  { 
560    scelta = 99;
561    lcd.clear();
562    lcd.print(" Exit = 0  ");
563    lcd.setCursor(0,1);
564    lcd.print("cont. key not 0");
565    if (Read_RC()==0)  {break;}
566    do {     
567      lcd.clear();
568      lcd.print(" choose column  ");
569      lcd.setCursor(0,1);
570      lcd.print("   from 1 to 7  ");
571      Serial.println("choose column [1-7]...");
572      col=Read_RC();
573      if((col<1)||(col>7))  {
574        lcd.clear();
575        lcd.print("column not valid"); 
576        Serial.println("column  is not valid");
577        delay(500);}
578    } while((col<1)||(col>7));
579    do  {     
580      lcd.clear();
581      lcd.print("   choose row   ");
582      lcd.setCursor(0,1);
583      lcd.print("   from 1 to 6  ");
584      Serial.println("choose row [1-6]...");
585      row=Read_RC();
586      if((row<1)||(row>6)) {
587        lcd.clear();
588        lcd.print("row not valid"); 
589        Serial.println("row is not valid");
590        delay(500);}
591    } while((row<1)||(row>6)); 
592    row--;
593    col--;
594    int calx = FourPos[row][col][0];
595    int caly = FourPos[row][col][1];
596    int calz = FourPos[row][col][2];
597    while (scelta!=10) {
598      lcd.clear();
599      lcd.print("CH+=OK CH-=Exit"); 
600      lcd.setCursor(0,1);
601      lcd.print("x=   y=    z=   ");
602      lcd.setCursor(2,1);
603      lcd.print(calx);
604      lcd.setCursor(7,1);
605      lcd.print(caly);
606      lcd.setCursor(13,1);
607      lcd.print(calz);
608      scelta=Read_RC();    
609      switch(scelta) {
610        case 1: calx--;  break;
611        case 3: calx++; break;
612        case 4: caly--; break;
613        case  6: caly++; break;
614        case 7: calz--; break;
615        case 9: calz++; break;
616        case 11:
617          FourPos[row][col][0] = calx;
618          FourPos[row][col][1]  = caly;
619          FourPos[row][col][2] = calz;
620          Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),calx,caly,calz,90+degrees(atan2(-calx,abs(caly))));
621          break;
622      }
623    }
624  }  
625}  
626
627void ShowDefeat() {
628  Set_Arm(0,130,100);
629  for(int k=0; k<=180*2; k++) {
630    MyServoWrite(SERVO6,  abs(180*sin(radians(k)))); 
631    delay(10);}
632  }
633
634void ShowWin() {
635  Set_Arm(0,100,300);
636  for(int k=0; k<=180*3; k ++){
637    MyServoWrite(SERVO6,  abs(180*sin(radians(k)))); 
638    delay(4);}}
639

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include  <IRremote2.h>
#include <LiquidCrystal.h>
int StartPos [3] = {0,-290,240};
int  FourPos [6][7][3] = {  { { 95,150, 65},{ 60,145, 75},{ 28,135, 70},{  0,130, 70},{  -25,130, 70},{ -58,125, 75},{ -90,120, 65} },                   //x,y,z
                           {  { 95,185, 70},{ 65,185, 70},{ 35,185, 70},{  0,180, 70},{ -30,175, 70},{ -63,170,  65},{ -95,165, 65} },
                           { { 95,210, 75},{ 65,220, 75},{  35,220, 75},{  0,208, 70},{ -30,213, 75},{ -65,208, 70},{-100,203, 70} },
                           {  { 95,250, 80},{ 65,250, 80},{ 35,245, 80},{  0,245, 80},{ -28,245, 80},{ -63,245,  80},{ -97,235, 75} },
                           { { 95,290, 90},{ 65,285, 90},{  35,285, 90},{  5,280, 90},{ -30,285, 90},{ -63,280, 90},{ -98,280, 90} },
                           {  {103,325,100},{ 68,325,100},{ 35,325,100},{  5,320,100},{ -25,325,100},{ -63,323,100},{  -98,318, 90} }, };
#define SKILL 10
#define ROW 6
#define COLUMN 7
  
int board[ROW*COLUMN];
long int depth,skill;
unsigned long int nodes;
/*  ---------- IR VARIABLE ---------- */
// RECV_PIN = 52;
IRrecv irrecv(52);
decode_results  results;

/* ---------- LIQUIDCRYSTAL VARIABLE ---------- */
// pin lcd:  rs = 8, en = 9, d4 = 4, d5 = 5, d6 = 6, d7 = 7;
LiquidCrystal lcd(8, 9, 4, 5,  6, 7);

/* ---------- ROBOTIC ARM VARIABLE ---------- */
Adafruit_PWMServoDriver  pwm = Adafruit_PWMServoDriver();
#define BASE_HGT 115 //base height
/*#define  HUMERUS  105   shoulder-to-elbow "bone"
  #define ULNA     100   elbow-to-wrist    "bone"
  #define GRIPPER  170   wrist-to-gripper  "bone"   */
#define  SERVO1  1
#define SERVO2  2
#define SERVO3  3
#define SERVO4  4
#define  SERVO5  5
#define SERVO6  6

#define SERVOMIN1  140
#define SERVOMAX1  475
#define SERVOMIN2  120
#define SERVOMAX2  415
#define SERVOMIN3  170
#define  SERVOMAX3  465
#define SERVOMIN4  140
#define SERVOMAX4  485
#define SERVOMIN5  105
#define SERVOMAX5  415
#define SERVOMIN6  220
#define SERVOMAX6  355
unsigned  int ServoMinMax[6][2] = {{SERVOMIN1,SERVOMAX1},
                                  {SERVOMIN2,SERVOMAX2},
                                  {SERVOMIN3,SERVOMAX3},
                                  {SERVOMIN4,SERVOMAX4},
                                  {SERVOMIN5,SERVOMAX5},
                                  {SERVOMIN6,SERVOMAX6}};

float  x1,x2,x3,x4,x5,x6 = 90;                          // servo position
float oldx1,oldx2,oldx3,oldx4,oldx5,oldx6  = x1;        // old servo position

float xaxis =   0;
float yaxis = 150;
float  zaxis = 100;

bool checkmin = true;
bool checkmax = true;

/* ----------  setup ---------- */
void setup() {
  Serial.begin(115200);
  randomSeed(analogRead(A0));
  lcd.begin(16, 2);
  lcd.clear();
  lcd.print(">> 4 in a row <<");
  lcd.setCursor(0,1);
  lcd.print("   by Danny003");
  delay(2000);
  pwm.begin();
  pwm.setPWMFreq(50);
  MyServoWrite(SERVO2,90);
  delay(1000);
  Set_Arm(xaxis,yaxis,zaxis);
  irrecv.enableIRIn();
  lcd.clear();
  delay(1000);
  ShowWin();
  delay(1000);
  ShowDefeat();
  delay(1000);
  Set_Arm(xaxis,yaxis,zaxis);
  lcd.print("Vuoi calibrare ? ");
  lcd.setCursor(0,1);
  lcd.print(" 1  = SI   0 = No");
  if ( Read_RC() == 1 ) { CalibraPos() ; }
}

void  loop() {
  int move,j,i,coins;
  coins = ROW*COLUMN;
  skill = SKILL-5;
  int choice = 0;
  for (i=0; i<(ROW*COLUMN); i++) {board[i] = 0;}
  lcd.clear();
  lcd.print("Choose Who Start");
  lcd.setCursor(0,1);
  lcd.print("CH+=Comp  CH-=You");
  Serial.println("Choose Who Start");
  Serial.println("CH+=Comp  CH-=You");
  choice = Read_RC();   
  if (choice==10) { 
    display_board();
    while(coins!=0)  {
      if (coins==34) {skill=SKILL-4;} /* quick start */
      if (coins==30) {skill=SKILL-3;} /* quick start */
      if (coins==26)  {skill=SKILL-2;} /* quick start */
      if (coins==22) {skill=SKILL-1;} /* quick  start */
      if (coins==18) {skill=SKILL;  } /* to  maximum */
      do  {     
        lcd.clear();
        lcd.print(" choose column  ");
        lcd.setCursor(0,1);
        lcd.print("   from 1 to 7  ");
        Serial.println("choose column  [1-7]...");
        move=Read_RC();
        if (board[move-1]!=0) {
          lcd.clear();
          lcd.print("column is full");
          Serial.println("column is  full");
          delay(500); }
        if ((move<1)||(move>7)) {
          lcd.clear();
          lcd.print("column not valid"); 
          Serial.println("column  is not valid");
          delay(500); }
      } while((board[move-1]!=0)||(move<1)||(move>7));
      lcd.clear();
      lcd.print("You moved in ");
      lcd.setCursor(13,0);
      lcd.print(move);
      lcd.setCursor(0,1);
      lcd.print("I am thinking...");  
      move--;
      j = ROW-1;
      while ((board[move+j*COLUMN]!=0)&&(j>=0))  {j--;}
      board[move+j*COLUMN] = 1;
      coins--;
      display_board();
      if(checkwin(1,move,4)) {
        lcd.clear();
        lcd.print("    Player  ");
        lcd.setCursor(0,1);
        lcd.print("    W I N S   ");
        Serial.println("You win\
");
        ShowDefeat();
        delay(3000);
        break;
      }
      move = best_move(-1);
      j = ROW-1;
      while ((board[move+j*COLUMN]!=0)&&(j>=0)) {j--;}
      board[move+j*COLUMN]  = -1;
      Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),FourPos[j][move][0],FourPos[j][move][1],FourPos[j][move][2],90+degrees(atan2(-FourPos[j][move][0],abs(FourPos[j][move][1]))));
      display_board();
      lcd.clear();
      lcd.print(" CPU move in ");
      lcd.print(move+1);
      Serial.println("CPU move in ");
      Serial.print(move+1);
      coins--;
      if (checkwin(-1,move,4)) {
        display_board();
        lcd.clear();
        lcd.print("    Computer  ");
        lcd.setCursor(0,1);
        lcd.print("    W I N S   ");
        Serial.println("CPU wins\
");
        ShowWin();
        delay(3000);
        break;
      }
      Serial.println("");
    }
    Serial.println("");
  }
  else if (choice == 11) {
    display_board();
    while(coins!=0) {
      if (coins==34) skill=SKILL-4; /* quick start */
      if (coins==30) skill=SKILL-3; /* quick start */
      if (coins==26) skill=SKILL-2;  /* quick start */
      if (coins==22) skill=SKILL-1; /* quick start */
      if  (coins==18) skill=SKILL;   /* ...to maximum */
      if (coins==ROW*COLUMN) {move=(random(0,8));}
      else  { move = best_move(-1); }
      j = ROW-1;
      while ((board[move+j*COLUMN]!=0)&&(j>=0))  {j--;}
      board[move+j*COLUMN] = -1;
      display_board();
      Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),FourPos[j][move][0],FourPos[j][move][1],FourPos[j][move][2],90+degrees(atan2(-FourPos[j][move][0],abs(FourPos[j][move][1]))));
      lcd.clear();
      lcd.print(" CPU move in ");
      lcd.print(move+1);
      Serial.println("CPU move in ");
      Serial.print(move+1);
      coins--;
      if (checkwin(-1,move,4)) {
        display_board();
        lcd.clear();
        lcd.print("    Computer  ");
        lcd.setCursor(0,1);
        lcd.print("    W I N S   ");
        Serial.println("CPU wins\
");
        ShowWin();
        delay(3000);
        break;
      }
      Serial.println("");
      do {     
        lcd.clear();
        lcd.print(" choose column  ");
        lcd.setCursor(0,1);
        lcd.print("   from 1 to 7  ");
        Serial.println("choose  column [1-7]...");
        move=Read_RC();
        if (board[move-1]!=0)  {
          lcd.clear();
          lcd.print("column is full");
          Serial.println("column  is full");
          delay(500);}
        if ((move<1)||(move>7)) {
          lcd.clear();
          lcd.print("column not valid"); 
          Serial.println("column  is not valid");
          delay(500);}
      } while ((board[move-1]!=0)||(move<1)||(move>7));
      lcd.clear();
      lcd.print("You moved in ");
      lcd.setCursor(13,0);
      lcd.print(move);
      lcd.setCursor(0,1);
      lcd.print("I am thinking...");  
      move--;
      j = ROW-1;
      while ((board[move+j*COLUMN]!=0)&&(j>=0))  {j--;}
      board[move+j*COLUMN] = 1;
      coins--;
      display_board();
      if (checkwin(1,move,4)) {
        Serial.println("You win\
");
        lcd.clear();
        lcd.print("    Player  ");
        lcd.setCursor(0,1);
        lcd.print("    W I N S   ");
        ShowDefeat();
        delay(3000);
        break;  
      }
    
      Serial.println("");
    }  
  }
}

/*  ---------- MyServoWrite ---------- */
/* This funciont is used to move a singular  servo*/
void MyServoWrite(int servo, float gradi) {
     pwm.setPWM( servo,  0, MapNew(gradi, 0.0, 180.0, ServoMinMax[servo-1][0], ServoMinMax[servo-1][1] ));
}

/*  ---------- MyServoWriteAll ---------- */
/* This funciont is used to move the  first 4 servo, it has been created to avoid to repeat the funciont MyServoWrite  4 times */
void MyServoWriteAll(float y1,float y2,float y3,float y4) {
  pwm.setPWM(  SERVO1, 0, MapNew(y1, 0.0, 180.0, ServoMinMax[SERVO1-1][0], ServoMinMax[SERVO1-1][1]  ));
  pwm.setPWM( SERVO2, 0, MapNew(y2, 0.0, 180.0, ServoMinMax[SERVO2-1][0],  ServoMinMax[SERVO2-1][1] ));
  pwm.setPWM( SERVO3, 0, MapNew(y3, 0.0, 180.0,  ServoMinMax[SERVO3-1][0], ServoMinMax[SERVO3-1][1] ));
  pwm.setPWM( SERVO4,  0, MapNew(y4, 0.0, 180.0, ServoMinMax[SERVO4-1][0], ServoMinMax[SERVO4-1][1] ));
}

/*  ---------- MyServoWriteGradual ---------- */
/*This function is similar to MyServoWriteAll;  but to avoid too rapid movement, 
 in particular when the robotic arm goes from  a position to another one,this function creates 
 a smooth and simultaneous movement  of the 4 servo, whose time depends on the variation of the bigger angle*/
void  MyServoWriteGradual( float newpos1, float newpos2, float newpos3, float newpos4  ) {
  float oldpos1 = oldx1;
  float oldpos2 = oldx2;
  float oldpos3 =  oldx3;
  float oldpos4 = oldx4;
  float differ1 = newpos1 - oldpos1;
  float  differ2 = newpos2 - oldpos2;
  float differ3 = newpos3 - oldpos3;
  float  differ4 = newpos4 - oldpos4;
  float largest = max(abs(differ1),max(abs(differ2),  max(abs(differ3),abs(differ4))));
  for ( int i = 0; i <= largest; i++ ) {
     oldpos1 += (differ1/largest);
     oldpos2 += (differ2/largest);
     oldpos3  += (differ3/largest);
     oldpos4 += (differ4/largest);
     MyServoWriteAll(oldpos1,oldpos2,oldpos3,oldpos4);
     delay(10);       
  } 
  MyServoWriteAll(newpos1,newpos2,newpos3,newpos4);
}  
 
/* ---------- Set_Arm ---------- */
/* This function is the most important  for the movement of the robotic arm, 
   because it calculates the postions of  the first 4 servo from the three coordinates x,y,z(taken from TakePos and TrisPos).
   The following code is specific to the configuration of MY robotic arm, so if  you want to use it, you have to adapt it to your arm.*/
void Set_Arm( float x,  float y, float z) {
  oldx1 = x1;
  oldx2 = x2;
  oldx3 = x3;
  oldx4  = x4;
  z = z-BASE_HGT;
  float dist_y_z = sqrt( sq(z) +  sq(x) + sq(y) );  
  if ( dist_y_z < 120.0 ) { checkmin=false; } 
    else { checkmin=true;  }
  if ( dist_y_z > 370.0 ) { checkmax=false; } 
    else { checkmax=true;  }
  if ( checkmin && checkmax ) {
    float alfa = 180.0-(degrees(acos((-27500  + sqrt( sq(27500) - 71400*(14225 - sq(dist_y_z))))/71400)));
    float gamma  = degrees(acos((27875 -sq(dist_y_z) - (34000*cos(radians(alfa))))/(-210*dist_y_z)));
    float omega = degrees(acos(sqrt(sq(x)+sq(y))/dist_y_z));
    x1 = 90.0-degrees(atan2(-x,abs(y)));
    if ( z > 0.0 ) { x2=180.0-(gamma+omega); }
      else { x2=180.0-(gamma-omega);  }  
    if ( y < 0.0) { x3=x4=(alfa-90.0); }
      else {
        x3=x4=(270.0-alfa);
        x2=180.0-x2;
        x1=180.0-x1;
      }
    if ((abs(oldx1-x1)+abs(oldx2-x2)+abs(oldx3-x3)+abs(oldx4-x4))  > 10.0) { MyServoWriteGradual(x1,x2,x3,x4); }
      else { MyServoWriteAll(x1,x2,x3,x4);  }
  } 
}


/* ---------- Reach_Position ---------- */
/*Through  this function, the Robotic Arm grabs a pawn  from a position and it leaves the pawn  in another position  */
void Reach_Position ( float fromx, float fromy, float  fromz, float fromangle, float tox, float toy, float toz, float toangle ) {
  Serial.println("test");
  Serial.println(tox);
  Serial.println(toy);
  Serial.println(toz);
  Serial.println(toangle);
  Set_Arm( fromx, fromy+50, fromz+50.0 );
  delay(300);
  MyServoWrite( SERVO6,  0 );
  MyServoWrite( SERVO5, fromangle );
  delay(1000);
  Set_Arm( fromx,  fromy+50, fromz );
  delay(300);
  Set_Arm( fromx, fromy, fromz );
  delay(300);
  MyServoWrite( SERVO6, 165 );
  delay(500);
  Set_Arm( fromx, fromy+100,  fromz );
  delay(500);
  Set_Arm( tox, toy, toz+150.0);
  delay(500);
  MyServoWrite( SERVO5, toangle );
  delay(300);
  Set_Arm( tox, toy, toz+50.0);
  delay(500);
  Set_Arm( tox, toy, toz );
  delay(300);
  MyServoWrite(  SERVO6, 10 );
  delay(500);
  Set_Arm( tox, toy, toz + 50.0);
  delay(500);
  Set_Arm( xaxis = 0.0, yaxis = 130.0 , zaxis = 200.0 );
  MyServoWrite( SERVO5,  90 );
  }

/* ---------- MapNew ---------- */
/*This function has been  created because the Arduino map() has not the funciont  constrain() and it does  not return a int value */
int MapNew( float x, float in_min, float in_max, float  out_min, float out_max ) {
   return round((constrain(x,in_min,in_max) - in_min)  * (out_max - out_min) / (in_max - in_min) + out_min);
}
 
/* ----------  Check_Win ---------- */
/*This function check all the 8 possible combinations  to win at tris. */

int checkwin(int player,int column,int lenght) {   
  long int j,r,l,i,height;
    lenght--;
    i = column;
    j = ROW-1;
    while(board[i+j*COLUMN]!=0) j--;
    j++;
    height = j;
 
    r  = 0;
    l = 0;
    while(((++i)<COLUMN)&&(board[i+j*COLUMN]==player)) r++;
    i = column;
    while(((--i)>=0)&&(board[i+j*COLUMN]==player)) l++;
    if  ((r+l)>=lenght) return 1;
    i = column;
 
    r = 0;
    if((j<0)){r--;}
    while(((++j)<ROW)&&(board[i+j*COLUMN]==player)) r++;
    if (r>=lenght) return  1;
    j = height;
 
    r = 0;
    l = 0;
    while(((++i)<COLUMN)&&((++j)<ROW)&&(board[i+j*COLUMN]==player))  r++;
    i = column;
    j = height;
    while(((--i)>=0)&&((--j)>=0)&&(board[i+j*COLUMN]==player))  l++;
    if ((r+l)>=lenght) return 1;
    i = column;
    j = height;
  
    r = 0;
    l = 0;
    while(((++i)<COLUMN)&&((--j)>=0)&&(board[i+j*COLUMN]==player))  r++;
    i = column;
    j = height;
    while(((--i)>=0)&&((++j)<ROW)&&(board[i+j*COLUMN]==player))  l++;
    if ((r+l)>=lenght) return 1;
 
    return 0;
}


int  extimated_value(int player) {
    long int i,j,value,l; 
    value = 0;
    for(l=2;l<4;l++)
    {
        for(i=0;i<COLUMN;i++)
        {
            if(checkwin(player,i,l)) value = value + l;
        }
    }
  
    return value;
}

int goodness(int player,int depth,int column,int  trigger) {
  long int max,i,value,j;
    max = -200;
    if (checkwin(-player,column,4))  return -128;
    if (depth==0) return 0;
    for(i=0;i<COLUMN;i++)
    {
        if(board[i]==0)
        {
            j = ROW-1;
            while(board[i+j*COLUMN]!=0)  j--;
            board[i+j*COLUMN] = player;
            nodes++;
            value  = -goodness(-player,depth-1,i,-max)/2;
            board[i+j*COLUMN] = 0;
            if (value>max) max = value;
            if (value>trigger) return  max;
        }
    }
    return max;
}

int best_move(int player)
{
    long int i,j,max,value,best,same,trigger,old,att;
    max = -100;
    best  = -1;
    long int maxnodes = -1;
    for(i=0;i<COLUMN;i++)
    {
        if(board[i]==0)
        {
            nodes = 0;
            j = ROW-1;
            while((board[i+j*COLUMN]!=0)&&(j>=0))  j--;
            board[i+j*COLUMN] = player;
            value = -goodness(-player,skill,i,200);
            //printf("\
move %d goodness: %d   tree size for this move: %d nodes",i+1,value,nodes);
            board[i+j*COLUMN] = 0;
            if ((value == max) && (nodes>maxnodes))  {
          maxnodes=nodes;
        max = value;
                best  = i;}
      if (value>max) {
          maxnodes=nodes;
        max = value;
                best = i; } 

         
      }
    }
    if(best==-1)
    {
        for(i=0;i<COLUMN;i++) if(board[i]==0) return i;
    }
 
    return best;
}



/* ---------- Read_RC ---------- */
/* This  function read the button you pressed  */
int Read_RC(){
  int returnvalue=99;
  while (returnvalue==99){
    while (!irrecv.decode(&results)) {};
    switch(results.value){
      case 0xFFA25D: returnvalue = 10; break;     // CH-
      case 0xFFE21D:  returnvalue = 11; break;     // CH+
      case 0xFF6897: returnvalue =  0; break;     //  0
      case 0xFF30CF: returnvalue =  1; break;     //  1 
      case  0xFF18E7: returnvalue =  2; break;     //  2
      case 0xFF7A85: returnvalue  =  3; break;     //  3
      case 0xFF10EF: returnvalue =  4; break;     //  4
      case 0xFF38C7: returnvalue =  5; break;     //  5
      case 0xFF5AA5:  returnvalue =  6; break;     //  6
      case 0xFF42BD: returnvalue =  7; break;     //  7
      case 0xFF4AB5: returnvalue =  8; break;     //  8
      case  0xFF52AD: returnvalue =  9; break;     //  9     
    }
    irrecv.resume();      
  }
  return returnvalue;
}



void display_board(void)  {
  long int i,j;
  Serial.println("");
  for(j=0; j<ROW; j++) {
    for(i=0; i<COLUMN; i++) {
    if (board[i+j*COLUMN]== 1) {Serial.print("X  ");}
    if (board[i+j*COLUMN]==-1) {Serial.print("O ");}
    if (board[i+j*COLUMN]==  0) {Serial.print(". ");}
    }
    Serial.println("");
  }
  for(i=0;  i<(COLUMN*2)-1; i++) {Serial.print("-");}
  Serial.println("");
  for(i=0;  i<COLUMN; i++) {Serial.print(i+1),Serial.print(" ");}
  Serial.println("");
}

void  CalibraPos() {
  int scelta = 99;
  int col,row;
  while( scelta != 0 )  { 
    scelta = 99;
    lcd.clear();
    lcd.print(" Exit = 0  ");
    lcd.setCursor(0,1);
    lcd.print("cont. key not 0");
    if (Read_RC()==0)  {break;}
    do {     
      lcd.clear();
      lcd.print(" choose column  ");
      lcd.setCursor(0,1);
      lcd.print("   from 1 to 7  ");
      Serial.println("choose column [1-7]...");
      col=Read_RC();
      if((col<1)||(col>7))  {
        lcd.clear();
        lcd.print("column not valid"); 
        Serial.println("column  is not valid");
        delay(500);}
    } while((col<1)||(col>7));
    do  {     
      lcd.clear();
      lcd.print("   choose row   ");
      lcd.setCursor(0,1);
      lcd.print("   from 1 to 6  ");
      Serial.println("choose row [1-6]...");
      row=Read_RC();
      if((row<1)||(row>6)) {
        lcd.clear();
        lcd.print("row not valid"); 
        Serial.println("row is not valid");
        delay(500);}
    } while((row<1)||(row>6)); 
    row--;
    col--;
    int calx = FourPos[row][col][0];
    int caly = FourPos[row][col][1];
    int calz = FourPos[row][col][2];
    while (scelta!=10) {
      lcd.clear();
      lcd.print("CH+=OK CH-=Exit"); 
      lcd.setCursor(0,1);
      lcd.print("x=   y=    z=   ");
      lcd.setCursor(2,1);
      lcd.print(calx);
      lcd.setCursor(7,1);
      lcd.print(caly);
      lcd.setCursor(13,1);
      lcd.print(calz);
      scelta=Read_RC();    
      switch(scelta) {
        case 1: calx--;  break;
        case 3: calx++; break;
        case 4: caly--; break;
        case  6: caly++; break;
        case 7: calz--; break;
        case 9: calz++; break;
        case 11:
          FourPos[row][col][0] = calx;
          FourPos[row][col][1]  = caly;
          FourPos[row][col][2] = calz;
          Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),calx,caly,calz,90+degrees(atan2(-calx,abs(caly))));
          break;
      }
    }
  }  
}  

void ShowDefeat() {
  Set_Arm(0,130,100);
  for(int k=0; k<=180*2; k++) {
    MyServoWrite(SERVO6,  abs(180*sin(radians(k)))); 
    delay(10);}
  }

void ShowWin() {
  Set_Arm(0,100,300);
  for(int k=0; k<=180*3; k ++){
    MyServoWrite(SERVO6,  abs(180*sin(radians(k)))); 
    delay(4);}}

Robotic Arm Code

arduino

This is the arduino Code

1
2#include <Wire.h>
3#include <Adafruit_PWMServoDriver.h>
4#include
5  <IRremote2.h>
6#include <LiquidCrystal.h>
7int StartPos [3] = {0,-290,240};
8int
9  FourPos [6][7][3] = {  { { 95,150, 65},{ 60,145, 75},{ 28,135, 70},{  0,130, 70},{
10  -25,130, 70},{ -58,125, 75},{ -90,120, 65} },                   //x,y,z
11                           {
12  { 95,185, 70},{ 65,185, 70},{ 35,185, 70},{  0,180, 70},{ -30,175, 70},{ -63,170,
13  65},{ -95,165, 65} },
14                           { { 95,210, 75},{ 65,220, 75},{
15  35,220, 75},{  0,208, 70},{ -30,213, 75},{ -65,208, 70},{-100,203, 70} },
16                           {
17  { 95,250, 80},{ 65,250, 80},{ 35,245, 80},{  0,245, 80},{ -28,245, 80},{ -63,245,
18  80},{ -97,235, 75} },
19                           { { 95,290, 90},{ 65,285, 90},{
20  35,285, 90},{  5,280, 90},{ -30,285, 90},{ -63,280, 90},{ -98,280, 90} },
21                           {
22  {103,325,100},{ 68,325,100},{ 35,325,100},{  5,320,100},{ -25,325,100},{ -63,323,100},{
23  -98,318, 90} }, };
24#define SKILL 10
25#define ROW 6
26#define COLUMN 7
27
28  
29int board[ROW*COLUMN];
30long int depth,skill;
31unsigned long int nodes;
32/*
33  ---------- IR VARIABLE ---------- */
34// RECV_PIN = 52;
35IRrecv irrecv(52);
36decode_results
37  results;
38
39/* ---------- LIQUIDCRYSTAL VARIABLE ---------- */
40// pin lcd:
41  rs = 8, en = 9, d4 = 4, d5 = 5, d6 = 6, d7 = 7;
42LiquidCrystal lcd(8, 9, 4, 5,
43  6, 7);
44
45/* ---------- ROBOTIC ARM VARIABLE ---------- */
46Adafruit_PWMServoDriver
47  pwm = Adafruit_PWMServoDriver();
48#define BASE_HGT 115 //base height
49/*#define
50  HUMERUS  105   shoulder-to-elbow "bone"
51  #define ULNA     100   elbow-to-wrist
52    "bone"
53  #define GRIPPER  170   wrist-to-gripper  "bone"   */
54#define
55  SERVO1  1
56#define SERVO2  2
57#define SERVO3  3
58#define SERVO4  4
59#define
60  SERVO5  5
61#define SERVO6  6
62
63#define SERVOMIN1  140
64#define SERVOMAX1
65  475
66#define SERVOMIN2  120
67#define SERVOMAX2  415
68#define SERVOMIN3  170
69#define
70  SERVOMAX3  465
71#define SERVOMIN4  140
72#define SERVOMAX4  485
73#define SERVOMIN5
74  105
75#define SERVOMAX5  415
76#define SERVOMIN6  220
77#define SERVOMAX6  355
78unsigned
79  int ServoMinMax[6][2] = {{SERVOMIN1,SERVOMAX1},
80                                  {SERVOMIN2,SERVOMAX2},
81
82                                  {SERVOMIN3,SERVOMAX3},
83                                  {SERVOMIN4,SERVOMAX4},
84
85                                  {SERVOMIN5,SERVOMAX5},
86                                  {SERVOMIN6,SERVOMAX6}};
87
88float
89  x1,x2,x3,x4,x5,x6 = 90;                          // servo position
90float oldx1,oldx2,oldx3,oldx4,oldx5,oldx6
91  = x1;        // old servo position
92
93float xaxis =   0;
94float yaxis = 150;
95float
96  zaxis = 100;
97
98bool checkmin = true;
99bool checkmax = true;
100
101/* ----------
102  setup ---------- */
103void setup() {
104  Serial.begin(115200);
105  randomSeed(analogRead(A0));
106
107  lcd.begin(16, 2);
108  lcd.clear();
109  lcd.print(">> 4 in a row <<");
110
111  lcd.setCursor(0,1);
112  lcd.print("   by Danny003");
113  delay(2000);
114
115  pwm.begin();
116  pwm.setPWMFreq(50);
117  MyServoWrite(SERVO2,90);
118  delay(1000);
119
120  Set_Arm(xaxis,yaxis,zaxis);
121  irrecv.enableIRIn();
122  lcd.clear();
123  delay(1000);
124
125  ShowWin();
126  delay(1000);
127  ShowDefeat();
128  delay(1000);
129  Set_Arm(xaxis,yaxis,zaxis);
130
131  lcd.print("Vuoi calibrare ? ");
132  lcd.setCursor(0,1);
133  lcd.print(" 1
134  = SI   0 = No");
135  if ( Read_RC() == 1 ) { CalibraPos() ; }
136}
137
138void
139  loop() {
140  int move,j,i,coins;
141  coins = ROW*COLUMN;
142  skill = SKILL-5;
143
144  int choice = 0;
145  for (i=0; i<(ROW*COLUMN); i++) {board[i] = 0;}
146  lcd.clear();
147
148  lcd.print("Choose Who Start");
149  lcd.setCursor(0,1);
150  lcd.print("CH+=Comp
151  CH-=You");
152  Serial.println("Choose Who Start");
153  Serial.println("CH+=Comp
154  CH-=You");
155  choice = Read_RC();   
156  if (choice==10) { 
157    display_board();
158
159    while(coins!=0)  {
160      if (coins==34) {skill=SKILL-4;} /* quick start */
161
162      if (coins==30) {skill=SKILL-3;} /* quick start */
163      if (coins==26)
164  {skill=SKILL-2;} /* quick start */
165      if (coins==22) {skill=SKILL-1;} /* quick
166  start */
167      if (coins==18) {skill=SKILL;  } /* to  maximum */
168      do
169  {     
170        lcd.clear();
171        lcd.print(" choose column  ");
172        lcd.setCursor(0,1);
173
174        lcd.print("   from 1 to 7  ");
175        Serial.println("choose column
176  [1-7]...");
177        move=Read_RC();
178        if (board[move-1]!=0) {
179          lcd.clear();
180
181          lcd.print("column is full");
182          Serial.println("column is
183  full");
184          delay(500); }
185        if ((move<1)||(move>7)) {
186          lcd.clear();
187
188          lcd.print("column not valid"); 
189          Serial.println("column
190  is not valid");
191          delay(500); }
192      } while((board[move-1]!=0)||(move<1)||(move>7));
193
194      lcd.clear();
195      lcd.print("You moved in ");
196      lcd.setCursor(13,0);
197
198      lcd.print(move);
199      lcd.setCursor(0,1);
200      lcd.print("I am thinking...");
201  
202      move--;
203      j = ROW-1;
204      while ((board[move+j*COLUMN]!=0)&&(j>=0))
205  {j--;}
206      board[move+j*COLUMN] = 1;
207      coins--;
208      display_board();
209
210      if(checkwin(1,move,4)) {
211        lcd.clear();
212        lcd.print("    Player
213  ");
214        lcd.setCursor(0,1);
215        lcd.print("    W I N S   ");
216
217        Serial.println("You win\
218");
219        ShowDefeat();
220        delay(3000);
221
222        break;
223      }
224      move = best_move(-1);
225      j = ROW-1;
226
227      while ((board[move+j*COLUMN]!=0)&&(j>=0)) {j--;}
228      board[move+j*COLUMN]
229  = -1;
230      Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),FourPos[j][move][0],FourPos[j][move][1],FourPos[j][move][2],90+degrees(atan2(-FourPos[j][move][0],abs(FourPos[j][move][1]))));
231
232      display_board();
233      lcd.clear();
234      lcd.print(" CPU move in ");
235
236      lcd.print(move+1);
237      Serial.println("CPU move in ");
238      Serial.print(move+1);
239
240      coins--;
241      if (checkwin(-1,move,4)) {
242        display_board();
243
244        lcd.clear();
245        lcd.print("    Computer  ");
246        lcd.setCursor(0,1);
247
248        lcd.print("    W I N S   ");
249        Serial.println("CPU wins\
250");
251
252        ShowWin();
253        delay(3000);
254        break;
255      }
256      Serial.println("");
257
258    }
259    Serial.println("");
260  }
261  else if (choice == 11) {
262    display_board();
263
264    while(coins!=0) {
265      if (coins==34) skill=SKILL-4; /* quick start */
266
267      if (coins==30) skill=SKILL-3; /* quick start */
268      if (coins==26) skill=SKILL-2;
269  /* quick start */
270      if (coins==22) skill=SKILL-1; /* quick start */
271      if
272  (coins==18) skill=SKILL;   /* ...to maximum */
273      if (coins==ROW*COLUMN) {move=(random(0,8));}
274
275      else  { move = best_move(-1); }
276      j = ROW-1;
277      while ((board[move+j*COLUMN]!=0)&&(j>=0))
278  {j--;}
279      board[move+j*COLUMN] = -1;
280      display_board();
281      Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),FourPos[j][move][0],FourPos[j][move][1],FourPos[j][move][2],90+degrees(atan2(-FourPos[j][move][0],abs(FourPos[j][move][1]))));
282
283      lcd.clear();
284      lcd.print(" CPU move in ");
285      lcd.print(move+1);
286
287      Serial.println("CPU move in ");
288      Serial.print(move+1);
289      coins--;
290
291      if (checkwin(-1,move,4)) {
292        display_board();
293        lcd.clear();
294
295        lcd.print("    Computer  ");
296        lcd.setCursor(0,1);
297        lcd.print("
298    W I N S   ");
299        Serial.println("CPU wins\
300");
301        ShowWin();
302
303        delay(3000);
304        break;
305      }
306      Serial.println("");
307
308      do {     
309        lcd.clear();
310        lcd.print(" choose column  ");
311
312        lcd.setCursor(0,1);
313        lcd.print("   from 1 to 7  ");
314        Serial.println("choose
315  column [1-7]...");
316        move=Read_RC();
317        if (board[move-1]!=0)
318  {
319          lcd.clear();
320          lcd.print("column is full");
321          Serial.println("column
322  is full");
323          delay(500);}
324        if ((move<1)||(move>7)) {
325          lcd.clear();
326
327          lcd.print("column not valid"); 
328          Serial.println("column
329  is not valid");
330          delay(500);}
331      } while ((board[move-1]!=0)||(move<1)||(move>7));
332
333      lcd.clear();
334      lcd.print("You moved in ");
335      lcd.setCursor(13,0);
336
337      lcd.print(move);
338      lcd.setCursor(0,1);
339      lcd.print("I am thinking...");
340  
341      move--;
342      j = ROW-1;
343      while ((board[move+j*COLUMN]!=0)&&(j>=0))
344  {j--;}
345      board[move+j*COLUMN] = 1;
346      coins--;
347      display_board();
348
349      if (checkwin(1,move,4)) {
350        Serial.println("You win\
351");
352        lcd.clear();
353
354        lcd.print("    Player  ");
355        lcd.setCursor(0,1);
356        lcd.print("
357    W I N S   ");
358        ShowDefeat();
359        delay(3000);
360        break;
361  
362      }
363    
364      Serial.println("");
365    }  
366  }
367}
368
369/*
370  ---------- MyServoWrite ---------- */
371/* This funciont is used to move a singular
372  servo*/
373void MyServoWrite(int servo, float gradi) {
374     pwm.setPWM( servo,
375  0, MapNew(gradi, 0.0, 180.0, ServoMinMax[servo-1][0], ServoMinMax[servo-1][1] ));
376}
377
378/*
379  ---------- MyServoWriteAll ---------- */
380/* This funciont is used to move the
381  first 4 servo, it has been created to avoid to repeat the funciont MyServoWrite
382  4 times */
383void MyServoWriteAll(float y1,float y2,float y3,float y4) {
384  pwm.setPWM(
385  SERVO1, 0, MapNew(y1, 0.0, 180.0, ServoMinMax[SERVO1-1][0], ServoMinMax[SERVO1-1][1]
386  ));
387  pwm.setPWM( SERVO2, 0, MapNew(y2, 0.0, 180.0, ServoMinMax[SERVO2-1][0],
388  ServoMinMax[SERVO2-1][1] ));
389  pwm.setPWM( SERVO3, 0, MapNew(y3, 0.0, 180.0,
390  ServoMinMax[SERVO3-1][0], ServoMinMax[SERVO3-1][1] ));
391  pwm.setPWM( SERVO4,
392  0, MapNew(y4, 0.0, 180.0, ServoMinMax[SERVO4-1][0], ServoMinMax[SERVO4-1][1] ));
393}
394
395/*
396  ---------- MyServoWriteGradual ---------- */
397/*This function is similar to MyServoWriteAll;
398  but to avoid too rapid movement, 
399 in particular when the robotic arm goes from
400  a position to another one,this function creates 
401 a smooth and simultaneous movement
402  of the 4 servo, whose time depends on the variation of the bigger angle*/
403void
404  MyServoWriteGradual( float newpos1, float newpos2, float newpos3, float newpos4
405  ) {
406  float oldpos1 = oldx1;
407  float oldpos2 = oldx2;
408  float oldpos3 =
409  oldx3;
410  float oldpos4 = oldx4;
411  float differ1 = newpos1 - oldpos1;
412  float
413  differ2 = newpos2 - oldpos2;
414  float differ3 = newpos3 - oldpos3;
415  float
416  differ4 = newpos4 - oldpos4;
417  float largest = max(abs(differ1),max(abs(differ2),
418  max(abs(differ3),abs(differ4))));
419  for ( int i = 0; i <= largest; i++ ) {
420
421     oldpos1 += (differ1/largest);
422     oldpos2 += (differ2/largest);
423     oldpos3
424  += (differ3/largest);
425     oldpos4 += (differ4/largest);
426     MyServoWriteAll(oldpos1,oldpos2,oldpos3,oldpos4);
427
428     delay(10);       
429  } 
430  MyServoWriteAll(newpos1,newpos2,newpos3,newpos4);
431}
432  
433 
434/* ---------- Set_Arm ---------- */
435/* This function is the most important
436  for the movement of the robotic arm, 
437   because it calculates the postions of
438  the first 4 servo from the three coordinates x,y,z(taken from TakePos and TrisPos).
439
440   The following code is specific to the configuration of MY robotic arm, so if
441  you want to use it, you have to adapt it to your arm.*/
442void Set_Arm( float x,
443  float y, float z) {
444  oldx1 = x1;
445  oldx2 = x2;
446  oldx3 = x3;
447  oldx4
448  = x4;
449  z = z-BASE_HGT;
450  float dist_y_z = sqrt( sq(z) +  sq(x) + sq(y) );
451  
452  if ( dist_y_z < 120.0 ) { checkmin=false; } 
453    else { checkmin=true;
454  }
455  if ( dist_y_z > 370.0 ) { checkmax=false; } 
456    else { checkmax=true;
457  }
458  if ( checkmin && checkmax ) {
459    float alfa = 180.0-(degrees(acos((-27500
460  + sqrt( sq(27500) - 71400*(14225 - sq(dist_y_z))))/71400)));
461    float gamma
462  = degrees(acos((27875 -sq(dist_y_z) - (34000*cos(radians(alfa))))/(-210*dist_y_z)));
463
464    float omega = degrees(acos(sqrt(sq(x)+sq(y))/dist_y_z));
465    x1 = 90.0-degrees(atan2(-x,abs(y)));
466
467    if ( z > 0.0 ) { x2=180.0-(gamma+omega); }
468      else { x2=180.0-(gamma-omega);
469  }  
470    if ( y < 0.0) { x3=x4=(alfa-90.0); }
471      else {
472        x3=x4=(270.0-alfa);
473
474        x2=180.0-x2;
475        x1=180.0-x1;
476      }
477    if ((abs(oldx1-x1)+abs(oldx2-x2)+abs(oldx3-x3)+abs(oldx4-x4))
478  > 10.0) { MyServoWriteGradual(x1,x2,x3,x4); }
479      else { MyServoWriteAll(x1,x2,x3,x4);
480  }
481  } 
482}
483
484
485/* ---------- Reach_Position ---------- */
486/*Through
487  this function, the Robotic Arm grabs a pawn  from a position and it leaves the pawn
488  in another position  */
489void Reach_Position ( float fromx, float fromy, float
490  fromz, float fromangle, float tox, float toy, float toz, float toangle ) {
491  Serial.println("test");
492
493  Serial.println(tox);
494  Serial.println(toy);
495  Serial.println(toz);
496  Serial.println(toangle);
497
498  Set_Arm( fromx, fromy+50, fromz+50.0 );
499  delay(300);
500  MyServoWrite( SERVO6,
501  0 );
502  MyServoWrite( SERVO5, fromangle );
503  delay(1000);
504  Set_Arm( fromx,
505  fromy+50, fromz );
506  delay(300);
507  Set_Arm( fromx, fromy, fromz );
508  delay(300);
509
510  MyServoWrite( SERVO6, 165 );
511  delay(500);
512  Set_Arm( fromx, fromy+100,
513  fromz );
514  delay(500);
515  Set_Arm( tox, toy, toz+150.0);
516  delay(500);
517
518  MyServoWrite( SERVO5, toangle );
519  delay(300);
520  Set_Arm( tox, toy, toz+50.0);
521
522  delay(500);
523  Set_Arm( tox, toy, toz );
524  delay(300);
525  MyServoWrite(
526  SERVO6, 10 );
527  delay(500);
528  Set_Arm( tox, toy, toz + 50.0);
529  delay(500);
530
531  Set_Arm( xaxis = 0.0, yaxis = 130.0 , zaxis = 200.0 );
532  MyServoWrite( SERVO5,
533  90 );
534  }
535
536/* ---------- MapNew ---------- */
537/*This function has been
538  created because the Arduino map() has not the funciont  constrain() and it does
539  not return a int value */
540int MapNew( float x, float in_min, float in_max, float
541  out_min, float out_max ) {
542   return round((constrain(x,in_min,in_max) - in_min)
543  * (out_max - out_min) / (in_max - in_min) + out_min);
544}
545 
546/* ----------
547  Check_Win ---------- */
548/*This function check all the 8 possible combinations
549  to win at tris. */
550
551int checkwin(int player,int column,int lenght) {   
552
553  long int j,r,l,i,height;
554    lenght--;
555    i = column;
556    j = ROW-1;
557
558    while(board[i+j*COLUMN]!=0) j--;
559    j++;
560    height = j;
561 
562    r
563  = 0;
564    l = 0;
565    while(((++i)<COLUMN)&&(board[i+j*COLUMN]==player)) r++;
566
567    i = column;
568    while(((--i)>=0)&&(board[i+j*COLUMN]==player)) l++;
569    if
570  ((r+l)>=lenght) return 1;
571    i = column;
572 
573    r = 0;
574    if((j<0)){r--;}
575
576    while(((++j)<ROW)&&(board[i+j*COLUMN]==player)) r++;
577    if (r>=lenght) return
578  1;
579    j = height;
580 
581    r = 0;
582    l = 0;
583    while(((++i)<COLUMN)&&((++j)<ROW)&&(board[i+j*COLUMN]==player))
584  r++;
585    i = column;
586    j = height;
587    while(((--i)>=0)&&((--j)>=0)&&(board[i+j*COLUMN]==player))
588  l++;
589    if ((r+l)>=lenght) return 1;
590    i = column;
591    j = height;
592
593  
594    r = 0;
595    l = 0;
596    while(((++i)<COLUMN)&&((--j)>=0)&&(board[i+j*COLUMN]==player))
597  r++;
598    i = column;
599    j = height;
600    while(((--i)>=0)&&((++j)<ROW)&&(board[i+j*COLUMN]==player))
601  l++;
602    if ((r+l)>=lenght) return 1;
603 
604    return 0;
605}
606
607
608int
609  extimated_value(int player) {
610    long int i,j,value,l; 
611    value = 0;
612
613    for(l=2;l<4;l++)
614    {
615        for(i=0;i<COLUMN;i++)
616        {
617
618            if(checkwin(player,i,l)) value = value + l;
619        }
620    }
621
622  
623    return value;
624}
625
626int goodness(int player,int depth,int column,int
627  trigger) {
628  long int max,i,value,j;
629    max = -200;
630    if (checkwin(-player,column,4))
631  return -128;
632    if (depth==0) return 0;
633    for(i=0;i<COLUMN;i++)
634    {
635
636        if(board[i]==0)
637        {
638            j = ROW-1;
639            while(board[i+j*COLUMN]!=0)
640  j--;
641            board[i+j*COLUMN] = player;
642            nodes++;
643            value
644  = -goodness(-player,depth-1,i,-max)/2;
645            board[i+j*COLUMN] = 0;
646
647            if (value>max) max = value;
648            if (value>trigger) return
649  max;
650        }
651    }
652    return max;
653}
654
655int best_move(int player)
656{
657
658    long int i,j,max,value,best,same,trigger,old,att;
659    max = -100;
660    best
661  = -1;
662    long int maxnodes = -1;
663    for(i=0;i<COLUMN;i++)
664    {
665        if(board[i]==0)
666
667        {
668            nodes = 0;
669            j = ROW-1;
670            while((board[i+j*COLUMN]!=0)&&(j>=0))
671  j--;
672            board[i+j*COLUMN] = player;
673            value = -goodness(-player,skill,i,200);
674
675            //printf("\
676move %d goodness: %d   tree size for this move: %d nodes",i+1,value,nodes);
677
678            board[i+j*COLUMN] = 0;
679            if ((value == max) && (nodes>maxnodes))
680  {
681          maxnodes=nodes;
682        max = value;
683                best
684  = i;}
685      if (value>max) {
686          maxnodes=nodes;
687        max = value;
688
689                best = i; } 
690
691         
692      }
693    }
694    if(best==-1)
695
696    {
697        for(i=0;i<COLUMN;i++) if(board[i]==0) return i;
698    }
699 
700
701    return best;
702}
703
704
705
706/* ---------- Read_RC ---------- */
707/* This
708  function read the button you pressed  */
709int Read_RC(){
710  int returnvalue=99;
711
712  while (returnvalue==99){
713    while (!irrecv.decode(&results)) {};
714    switch(results.value){
715
716      case 0xFFA25D: returnvalue = 10; break;     // CH-
717      case 0xFFE21D:
718  returnvalue = 11; break;     // CH+
719      case 0xFF6897: returnvalue =  0; break;
720     //  0
721      case 0xFF30CF: returnvalue =  1; break;     //  1 
722      case
723  0xFF18E7: returnvalue =  2; break;     //  2
724      case 0xFF7A85: returnvalue
725  =  3; break;     //  3
726      case 0xFF10EF: returnvalue =  4; break;     //  4
727
728      case 0xFF38C7: returnvalue =  5; break;     //  5
729      case 0xFF5AA5:
730  returnvalue =  6; break;     //  6
731      case 0xFF42BD: returnvalue =  7; break;
732     //  7
733      case 0xFF4AB5: returnvalue =  8; break;     //  8
734      case
735  0xFF52AD: returnvalue =  9; break;     //  9     
736    }
737    irrecv.resume();
738      
739  }
740  return returnvalue;
741}
742
743
744
745void display_board(void)
746  {
747  long int i,j;
748  Serial.println("");
749  for(j=0; j<ROW; j++) {
750
751    for(i=0; i<COLUMN; i++) {
752    if (board[i+j*COLUMN]== 1) {Serial.print("X
753  ");}
754    if (board[i+j*COLUMN]==-1) {Serial.print("O ");}
755    if (board[i+j*COLUMN]==
756  0) {Serial.print(". ");}
757    }
758    Serial.println("");
759  }
760  for(i=0;
761  i<(COLUMN*2)-1; i++) {Serial.print("-");}
762  Serial.println("");
763  for(i=0;
764  i<COLUMN; i++) {Serial.print(i+1),Serial.print(" ");}
765  Serial.println("");
766}
767
768void
769  CalibraPos() {
770  int scelta = 99;
771  int col,row;
772  while( scelta != 0 )
773  { 
774    scelta = 99;
775    lcd.clear();
776    lcd.print(" Exit = 0  ");
777
778    lcd.setCursor(0,1);
779    lcd.print("cont. key not 0");
780    if (Read_RC()==0)
781  {break;}
782    do {     
783      lcd.clear();
784      lcd.print(" choose column
785  ");
786      lcd.setCursor(0,1);
787      lcd.print("   from 1 to 7  ");
788
789      Serial.println("choose column [1-7]...");
790      col=Read_RC();
791      if((col<1)||(col>7))
792  {
793        lcd.clear();
794        lcd.print("column not valid"); 
795        Serial.println("column
796  is not valid");
797        delay(500);}
798    } while((col<1)||(col>7));
799    do
800  {     
801      lcd.clear();
802      lcd.print("   choose row   ");
803      lcd.setCursor(0,1);
804
805      lcd.print("   from 1 to 6  ");
806      Serial.println("choose row [1-6]...");
807
808      row=Read_RC();
809      if((row<1)||(row>6)) {
810        lcd.clear();
811
812        lcd.print("row not valid"); 
813        Serial.println("row is not valid");
814
815        delay(500);}
816    } while((row<1)||(row>6)); 
817    row--;
818    col--;
819
820    int calx = FourPos[row][col][0];
821    int caly = FourPos[row][col][1];
822
823    int calz = FourPos[row][col][2];
824    while (scelta!=10) {
825      lcd.clear();
826
827      lcd.print("CH+=OK CH-=Exit"); 
828      lcd.setCursor(0,1);
829      lcd.print("x=
830   y=    z=   ");
831      lcd.setCursor(2,1);
832      lcd.print(calx);
833      lcd.setCursor(7,1);
834
835      lcd.print(caly);
836      lcd.setCursor(13,1);
837      lcd.print(calz);
838
839      scelta=Read_RC();    
840      switch(scelta) {
841        case 1: calx--;
842  break;
843        case 3: calx++; break;
844        case 4: caly--; break;
845        case
846  6: caly++; break;
847        case 7: calz--; break;
848        case 9: calz++; break;
849
850        case 11:
851          FourPos[row][col][0] = calx;
852          FourPos[row][col][1]
853  = caly;
854          FourPos[row][col][2] = calz;
855          Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),calx,caly,calz,90+degrees(atan2(-calx,abs(caly))));
856
857          break;
858      }
859    }
860  }  
861}  
862
863void ShowDefeat() {
864
865  Set_Arm(0,130,100);
866  for(int k=0; k<=180*2; k++) {
867    MyServoWrite(SERVO6,
868  abs(180*sin(radians(k)))); 
869    delay(10);}
870  }
871
872void ShowWin() {
873
874  Set_Arm(0,100,300);
875  for(int k=0; k<=180*3; k ++){
876    MyServoWrite(SERVO6,
877  abs(180*sin(radians(k)))); 
878    delay(4);}}
879

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include
  <IRremote2.h>
#include <LiquidCrystal.h>
int StartPos [3] = {0,-290,240};
int
  FourPos [6][7][3] = {  { { 95,150, 65},{ 60,145, 75},{ 28,135, 70},{  0,130, 70},{
  -25,130, 70},{ -58,125, 75},{ -90,120, 65} },                   //x,y,z
                           {
  { 95,185, 70},{ 65,185, 70},{ 35,185, 70},{  0,180, 70},{ -30,175, 70},{ -63,170,
  65},{ -95,165, 65} },
                           { { 95,210, 75},{ 65,220, 75},{
  35,220, 75},{  0,208, 70},{ -30,213, 75},{ -65,208, 70},{-100,203, 70} },
                           {
  { 95,250, 80},{ 65,250, 80},{ 35,245, 80},{  0,245, 80},{ -28,245, 80},{ -63,245,
  80},{ -97,235, 75} },
                           { { 95,290, 90},{ 65,285, 90},{
  35,285, 90},{  5,280, 90},{ -30,285, 90},{ -63,280, 90},{ -98,280, 90} },
                           {
  {103,325,100},{ 68,325,100},{ 35,325,100},{  5,320,100},{ -25,325,100},{ -63,323,100},{
  -98,318, 90} }, };
#define SKILL 10
#define ROW 6
#define COLUMN 7

  
int board[ROW*COLUMN];
long int depth,skill;
unsigned long int nodes;
/*
  ---------- IR VARIABLE ---------- */
// RECV_PIN = 52;
IRrecv irrecv(52);
decode_results
  results;

/* ---------- LIQUIDCRYSTAL VARIABLE ---------- */
// pin lcd:
  rs = 8, en = 9, d4 = 4, d5 = 5, d6 = 6, d7 = 7;
LiquidCrystal lcd(8, 9, 4, 5,
  6, 7);

/* ---------- ROBOTIC ARM VARIABLE ---------- */
Adafruit_PWMServoDriver
  pwm = Adafruit_PWMServoDriver();
#define BASE_HGT 115 //base height
/*#define
  HUMERUS  105   shoulder-to-elbow "bone"
  #define ULNA     100   elbow-to-wrist
    "bone"
  #define GRIPPER  170   wrist-to-gripper  "bone"   */
#define
  SERVO1  1
#define SERVO2  2
#define SERVO3  3
#define SERVO4  4
#define
  SERVO5  5
#define SERVO6  6

#define SERVOMIN1  140
#define SERVOMAX1
  475
#define SERVOMIN2  120
#define SERVOMAX2  415
#define SERVOMIN3  170
#define
  SERVOMAX3  465
#define SERVOMIN4  140
#define SERVOMAX4  485
#define SERVOMIN5
  105
#define SERVOMAX5  415
#define SERVOMIN6  220
#define SERVOMAX6  355
unsigned
  int ServoMinMax[6][2] = {{SERVOMIN1,SERVOMAX1},
                                  {SERVOMIN2,SERVOMAX2},

                                  {SERVOMIN3,SERVOMAX3},
                                  {SERVOMIN4,SERVOMAX4},

                                  {SERVOMIN5,SERVOMAX5},
                                  {SERVOMIN6,SERVOMAX6}};

float
  x1,x2,x3,x4,x5,x6 = 90;                          // servo position
float oldx1,oldx2,oldx3,oldx4,oldx5,oldx6
  = x1;        // old servo position

float xaxis =   0;
float yaxis = 150;
float
  zaxis = 100;

bool checkmin = true;
bool checkmax = true;

/* ----------
  setup ---------- */
void setup() {
  Serial.begin(115200);
  randomSeed(analogRead(A0));

  lcd.begin(16, 2);
  lcd.clear();
  lcd.print(">> 4 in a row <<");

  lcd.setCursor(0,1);
  lcd.print("   by Danny003");
  delay(2000);

  pwm.begin();
  pwm.setPWMFreq(50);
  MyServoWrite(SERVO2,90);
  delay(1000);

  Set_Arm(xaxis,yaxis,zaxis);
  irrecv.enableIRIn();
  lcd.clear();
  delay(1000);

  ShowWin();
  delay(1000);
  ShowDefeat();
  delay(1000);
  Set_Arm(xaxis,yaxis,zaxis);

  lcd.print("Vuoi calibrare ? ");
  lcd.setCursor(0,1);
  lcd.print(" 1
  = SI   0 = No");
  if ( Read_RC() == 1 ) { CalibraPos() ; }
}

void
  loop() {
  int move,j,i,coins;
  coins = ROW*COLUMN;
  skill = SKILL-5;

  int choice = 0;
  for (i=0; i<(ROW*COLUMN); i++) {board[i] = 0;}
  lcd.clear();

  lcd.print("Choose Who Start");
  lcd.setCursor(0,1);
  lcd.print("CH+=Comp
  CH-=You");
  Serial.println("Choose Who Start");
  Serial.println("CH+=Comp
  CH-=You");
  choice = Read_RC();   
  if (choice==10) { 
    display_board();

    while(coins!=0)  {
      if (coins==34) {skill=SKILL-4;} /* quick start */

      if (coins==30) {skill=SKILL-3;} /* quick start */
      if (coins==26)
  {skill=SKILL-2;} /* quick start */
      if (coins==22) {skill=SKILL-1;} /* quick
  start */
      if (coins==18) {skill=SKILL;  } /* to  maximum */
      do
  {     
        lcd.clear();
        lcd.print(" choose column  ");
        lcd.setCursor(0,1);

        lcd.print("   from 1 to 7  ");
        Serial.println("choose column
  [1-7]...");
        move=Read_RC();
        if (board[move-1]!=0) {
          lcd.clear();

          lcd.print("column is full");
          Serial.println("column is
  full");
          delay(500); }
        if ((move<1)||(move>7)) {
          lcd.clear();

          lcd.print("column not valid"); 
          Serial.println("column
  is not valid");
          delay(500); }
      } while((board[move-1]!=0)||(move<1)||(move>7));

      lcd.clear();
      lcd.print("You moved in ");
      lcd.setCursor(13,0);

      lcd.print(move);
      lcd.setCursor(0,1);
      lcd.print("I am thinking...");
  
      move--;
      j = ROW-1;
      while ((board[move+j*COLUMN]!=0)&&(j>=0))
  {j--;}
      board[move+j*COLUMN] = 1;
      coins--;
      display_board();

      if(checkwin(1,move,4)) {
        lcd.clear();
        lcd.print("    Player
  ");
        lcd.setCursor(0,1);
        lcd.print("    W I N S   ");

        Serial.println("You win\
");
        ShowDefeat();
        delay(3000);

        break;
      }
      move = best_move(-1);
      j = ROW-1;

      while ((board[move+j*COLUMN]!=0)&&(j>=0)) {j--;}
      board[move+j*COLUMN]
  = -1;
      Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),FourPos[j][move][0],FourPos[j][move][1],FourPos[j][move][2],90+degrees(atan2(-FourPos[j][move][0],abs(FourPos[j][move][1]))));

      display_board();
      lcd.clear();
      lcd.print(" CPU move in ");

      lcd.print(move+1);
      Serial.println("CPU move in ");
      Serial.print(move+1);

      coins--;
      if (checkwin(-1,move,4)) {
        display_board();

        lcd.clear();
        lcd.print("    Computer  ");
        lcd.setCursor(0,1);

        lcd.print("    W I N S   ");
        Serial.println("CPU wins\
");

        ShowWin();
        delay(3000);
        break;
      }
      Serial.println("");

    }
    Serial.println("");
  }
  else if (choice == 11) {
    display_board();

    while(coins!=0) {
      if (coins==34) skill=SKILL-4; /* quick start */

      if (coins==30) skill=SKILL-3; /* quick start */
      if (coins==26) skill=SKILL-2;
  /* quick start */
      if (coins==22) skill=SKILL-1; /* quick start */
      if
  (coins==18) skill=SKILL;   /* ...to maximum */
      if (coins==ROW*COLUMN) {move=(random(0,8));}

      else  { move = best_move(-1); }
      j = ROW-1;
      while ((board[move+j*COLUMN]!=0)&&(j>=0))
  {j--;}
      board[move+j*COLUMN] = -1;
      display_board();
      Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),FourPos[j][move][0],FourPos[j][move][1],FourPos[j][move][2],90+degrees(atan2(-FourPos[j][move][0],abs(FourPos[j][move][1]))));

      lcd.clear();
      lcd.print(" CPU move in ");
      lcd.print(move+1);

      Serial.println("CPU move in ");
      Serial.print(move+1);
      coins--;

      if (checkwin(-1,move,4)) {
        display_board();
        lcd.clear();

        lcd.print("    Computer  ");
        lcd.setCursor(0,1);
        lcd.print("
    W I N S   ");
        Serial.println("CPU wins\
");
        ShowWin();

        delay(3000);
        break;
      }
      Serial.println("");

      do {     
        lcd.clear();
        lcd.print(" choose column  ");

        lcd.setCursor(0,1);
        lcd.print("   from 1 to 7  ");
        Serial.println("choose
  column [1-7]...");
        move=Read_RC();
        if (board[move-1]!=0)
  {
          lcd.clear();
          lcd.print("column is full");
          Serial.println("column
  is full");
          delay(500);}
        if ((move<1)||(move>7)) {
          lcd.clear();

          lcd.print("column not valid"); 
          Serial.println("column
  is not valid");
          delay(500);}
      } while ((board[move-1]!=0)||(move<1)||(move>7));

      lcd.clear();
      lcd.print("You moved in ");
      lcd.setCursor(13,0);

      lcd.print(move);
      lcd.setCursor(0,1);
      lcd.print("I am thinking...");
  
      move--;
      j = ROW-1;
      while ((board[move+j*COLUMN]!=0)&&(j>=0))
  {j--;}
      board[move+j*COLUMN] = 1;
      coins--;
      display_board();

      if (checkwin(1,move,4)) {
        Serial.println("You win\
");
        lcd.clear();

        lcd.print("    Player  ");
        lcd.setCursor(0,1);
        lcd.print("
    W I N S   ");
        ShowDefeat();
        delay(3000);
        break;
  
      }
    
      Serial.println("");
    }  
  }
}

/*
  ---------- MyServoWrite ---------- */
/* This funciont is used to move a singular
  servo*/
void MyServoWrite(int servo, float gradi) {
     pwm.setPWM( servo,
  0, MapNew(gradi, 0.0, 180.0, ServoMinMax[servo-1][0], ServoMinMax[servo-1][1] ));
}

/*
  ---------- MyServoWriteAll ---------- */
/* This funciont is used to move the
  first 4 servo, it has been created to avoid to repeat the funciont MyServoWrite
  4 times */
void MyServoWriteAll(float y1,float y2,float y3,float y4) {
  pwm.setPWM(
  SERVO1, 0, MapNew(y1, 0.0, 180.0, ServoMinMax[SERVO1-1][0], ServoMinMax[SERVO1-1][1]
  ));
  pwm.setPWM( SERVO2, 0, MapNew(y2, 0.0, 180.0, ServoMinMax[SERVO2-1][0],
  ServoMinMax[SERVO2-1][1] ));
  pwm.setPWM( SERVO3, 0, MapNew(y3, 0.0, 180.0,
  ServoMinMax[SERVO3-1][0], ServoMinMax[SERVO3-1][1] ));
  pwm.setPWM( SERVO4,
  0, MapNew(y4, 0.0, 180.0, ServoMinMax[SERVO4-1][0], ServoMinMax[SERVO4-1][1] ));
}

/*
  ---------- MyServoWriteGradual ---------- */
/*This function is similar to MyServoWriteAll;
  but to avoid too rapid movement, 
 in particular when the robotic arm goes from
  a position to another one,this function creates 
 a smooth and simultaneous movement
  of the 4 servo, whose time depends on the variation of the bigger angle*/
void
  MyServoWriteGradual( float newpos1, float newpos2, float newpos3, float newpos4
  ) {
  float oldpos1 = oldx1;
  float oldpos2 = oldx2;
  float oldpos3 =
  oldx3;
  float oldpos4 = oldx4;
  float differ1 = newpos1 - oldpos1;
  float
  differ2 = newpos2 - oldpos2;
  float differ3 = newpos3 - oldpos3;
  float
  differ4 = newpos4 - oldpos4;
  float largest = max(abs(differ1),max(abs(differ2),
  max(abs(differ3),abs(differ4))));
  for ( int i = 0; i <= largest; i++ ) {

     oldpos1 += (differ1/largest);
     oldpos2 += (differ2/largest);
     oldpos3
  += (differ3/largest);
     oldpos4 += (differ4/largest);
     MyServoWriteAll(oldpos1,oldpos2,oldpos3,oldpos4);

     delay(10);       
  } 
  MyServoWriteAll(newpos1,newpos2,newpos3,newpos4);
}
  
 
/* ---------- Set_Arm ---------- */
/* This function is the most important
  for the movement of the robotic arm, 
   because it calculates the postions of
  the first 4 servo from the three coordinates x,y,z(taken from TakePos and TrisPos).

   The following code is specific to the configuration of MY robotic arm, so if
  you want to use it, you have to adapt it to your arm.*/
void Set_Arm( float x,
  float y, float z) {
  oldx1 = x1;
  oldx2 = x2;
  oldx3 = x3;
  oldx4
  = x4;
  z = z-BASE_HGT;
  float dist_y_z = sqrt( sq(z) +  sq(x) + sq(y) );
  
  if ( dist_y_z < 120.0 ) { checkmin=false; } 
    else { checkmin=true;
  }
  if ( dist_y_z > 370.0 ) { checkmax=false; } 
    else { checkmax=true;
  }
  if ( checkmin && checkmax ) {
    float alfa = 180.0-(degrees(acos((-27500
  + sqrt( sq(27500) - 71400*(14225 - sq(dist_y_z))))/71400)));
    float gamma
  = degrees(acos((27875 -sq(dist_y_z) - (34000*cos(radians(alfa))))/(-210*dist_y_z)));

    float omega = degrees(acos(sqrt(sq(x)+sq(y))/dist_y_z));
    x1 = 90.0-degrees(atan2(-x,abs(y)));

    if ( z > 0.0 ) { x2=180.0-(gamma+omega); }
      else { x2=180.0-(gamma-omega);
  }  
    if ( y < 0.0) { x3=x4=(alfa-90.0); }
      else {
        x3=x4=(270.0-alfa);

        x2=180.0-x2;
        x1=180.0-x1;
      }
    if ((abs(oldx1-x1)+abs(oldx2-x2)+abs(oldx3-x3)+abs(oldx4-x4))
  > 10.0) { MyServoWriteGradual(x1,x2,x3,x4); }
      else { MyServoWriteAll(x1,x2,x3,x4);
  }
  } 
}


/* ---------- Reach_Position ---------- */
/*Through
  this function, the Robotic Arm grabs a pawn  from a position and it leaves the pawn
  in another position  */
void Reach_Position ( float fromx, float fromy, float
  fromz, float fromangle, float tox, float toy, float toz, float toangle ) {
  Serial.println("test");

  Serial.println(tox);
  Serial.println(toy);
  Serial.println(toz);
  Serial.println(toangle);

  Set_Arm( fromx, fromy+50, fromz+50.0 );
  delay(300);
  MyServoWrite( SERVO6,
  0 );
  MyServoWrite( SERVO5, fromangle );
  delay(1000);
  Set_Arm( fromx,
  fromy+50, fromz );
  delay(300);
  Set_Arm( fromx, fromy, fromz );
  delay(300);

  MyServoWrite( SERVO6, 165 );
  delay(500);
  Set_Arm( fromx, fromy+100,
  fromz );
  delay(500);
  Set_Arm( tox, toy, toz+150.0);
  delay(500);

  MyServoWrite( SERVO5, toangle );
  delay(300);
  Set_Arm( tox, toy, toz+50.0);

  delay(500);
  Set_Arm( tox, toy, toz );
  delay(300);
  MyServoWrite(
  SERVO6, 10 );
  delay(500);
  Set_Arm( tox, toy, toz + 50.0);
  delay(500);

  Set_Arm( xaxis = 0.0, yaxis = 130.0 , zaxis = 200.0 );
  MyServoWrite( SERVO5,
  90 );
  }

/* ---------- MapNew ---------- */
/*This function has been
  created because the Arduino map() has not the funciont  constrain() and it does
  not return a int value */
int MapNew( float x, float in_min, float in_max, float
  out_min, float out_max ) {
   return round((constrain(x,in_min,in_max) - in_min)
  * (out_max - out_min) / (in_max - in_min) + out_min);
}
 
/* ----------
  Check_Win ---------- */
/*This function check all the 8 possible combinations
  to win at tris. */

int checkwin(int player,int column,int lenght) {   

  long int j,r,l,i,height;
    lenght--;
    i = column;
    j = ROW-1;

    while(board[i+j*COLUMN]!=0) j--;
    j++;
    height = j;
 
    r
  = 0;
    l = 0;
    while(((++i)<COLUMN)&&(board[i+j*COLUMN]==player)) r++;

    i = column;
    while(((--i)>=0)&&(board[i+j*COLUMN]==player)) l++;
    if
  ((r+l)>=lenght) return 1;
    i = column;
 
    r = 0;
    if((j<0)){r--;}

    while(((++j)<ROW)&&(board[i+j*COLUMN]==player)) r++;
    if (r>=lenght) return
  1;
    j = height;
 
    r = 0;
    l = 0;
    while(((++i)<COLUMN)&&((++j)<ROW)&&(board[i+j*COLUMN]==player))
  r++;
    i = column;
    j = height;
    while(((--i)>=0)&&((--j)>=0)&&(board[i+j*COLUMN]==player))
  l++;
    if ((r+l)>=lenght) return 1;
    i = column;
    j = height;

  
    r = 0;
    l = 0;
    while(((++i)<COLUMN)&&((--j)>=0)&&(board[i+j*COLUMN]==player))
  r++;
    i = column;
    j = height;
    while(((--i)>=0)&&((++j)<ROW)&&(board[i+j*COLUMN]==player))
  l++;
    if ((r+l)>=lenght) return 1;
 
    return 0;
}


int
  extimated_value(int player) {
    long int i,j,value,l; 
    value = 0;

    for(l=2;l<4;l++)
    {
        for(i=0;i<COLUMN;i++)
        {

            if(checkwin(player,i,l)) value = value + l;
        }
    }

  
    return value;
}

int goodness(int player,int depth,int column,int
  trigger) {
  long int max,i,value,j;
    max = -200;
    if (checkwin(-player,column,4))
  return -128;
    if (depth==0) return 0;
    for(i=0;i<COLUMN;i++)
    {

        if(board[i]==0)
        {
            j = ROW-1;
            while(board[i+j*COLUMN]!=0)
  j--;
            board[i+j*COLUMN] = player;
            nodes++;
            value
  = -goodness(-player,depth-1,i,-max)/2;
            board[i+j*COLUMN] = 0;

            if (value>max) max = value;
            if (value>trigger) return
  max;
        }
    }
    return max;
}

int best_move(int player)
{

    long int i,j,max,value,best,same,trigger,old,att;
    max = -100;
    best
  = -1;
    long int maxnodes = -1;
    for(i=0;i<COLUMN;i++)
    {
        if(board[i]==0)

        {
            nodes = 0;
            j = ROW-1;
            while((board[i+j*COLUMN]!=0)&&(j>=0))
  j--;
            board[i+j*COLUMN] = player;
            value = -goodness(-player,skill,i,200);

            //printf("\
move %d goodness: %d   tree size for this move: %d nodes",i+1,value,nodes);

            board[i+j*COLUMN] = 0;
            if ((value == max) && (nodes>maxnodes))
  {
          maxnodes=nodes;
        max = value;
                best
  = i;}
      if (value>max) {
          maxnodes=nodes;
        max = value;

                best = i; } 

         
      }
    }
    if(best==-1)

    {
        for(i=0;i<COLUMN;i++) if(board[i]==0) return i;
    }
 

    return best;
}



/* ---------- Read_RC ---------- */
/* This
  function read the button you pressed  */
int Read_RC(){
  int returnvalue=99;

  while (returnvalue==99){
    while (!irrecv.decode(&results)) {};
    switch(results.value){

      case 0xFFA25D: returnvalue = 10; break;     // CH-
      case 0xFFE21D:
  returnvalue = 11; break;     // CH+
      case 0xFF6897: returnvalue =  0; break;
     //  0
      case 0xFF30CF: returnvalue =  1; break;     //  1 
      case
  0xFF18E7: returnvalue =  2; break;     //  2
      case 0xFF7A85: returnvalue
  =  3; break;     //  3
      case 0xFF10EF: returnvalue =  4; break;     //  4

      case 0xFF38C7: returnvalue =  5; break;     //  5
      case 0xFF5AA5:
  returnvalue =  6; break;     //  6
      case 0xFF42BD: returnvalue =  7; break;
     //  7
      case 0xFF4AB5: returnvalue =  8; break;     //  8
      case
  0xFF52AD: returnvalue =  9; break;     //  9     
    }
    irrecv.resume();
      
  }
  return returnvalue;
}



void display_board(void)
  {
  long int i,j;
  Serial.println("");
  for(j=0; j<ROW; j++) {

    for(i=0; i<COLUMN; i++) {
    if (board[i+j*COLUMN]== 1) {Serial.print("X
  ");}
    if (board[i+j*COLUMN]==-1) {Serial.print("O ");}
    if (board[i+j*COLUMN]==
  0) {Serial.print(". ");}
    }
    Serial.println("");
  }
  for(i=0;
  i<(COLUMN*2)-1; i++) {Serial.print("-");}
  Serial.println("");
  for(i=0;
  i<COLUMN; i++) {Serial.print(i+1),Serial.print(" ");}
  Serial.println("");
}

void
  CalibraPos() {
  int scelta = 99;
  int col,row;
  while( scelta != 0 )
  { 
    scelta = 99;
    lcd.clear();
    lcd.print(" Exit = 0  ");

    lcd.setCursor(0,1);
    lcd.print("cont. key not 0");
    if (Read_RC()==0)
  {break;}
    do {     
      lcd.clear();
      lcd.print(" choose column
  ");
      lcd.setCursor(0,1);
      lcd.print("   from 1 to 7  ");

      Serial.println("choose column [1-7]...");
      col=Read_RC();
      if((col<1)||(col>7))
  {
        lcd.clear();
        lcd.print("column not valid"); 
        Serial.println("column
  is not valid");
        delay(500);}
    } while((col<1)||(col>7));
    do
  {     
      lcd.clear();
      lcd.print("   choose row   ");
      lcd.setCursor(0,1);

      lcd.print("   from 1 to 6  ");
      Serial.println("choose row [1-6]...");

      row=Read_RC();
      if((row<1)||(row>6)) {
        lcd.clear();

        lcd.print("row not valid"); 
        Serial.println("row is not valid");

        delay(500);}
    } while((row<1)||(row>6)); 
    row--;
    col--;

    int calx = FourPos[row][col][0];
    int caly = FourPos[row][col][1];

    int calz = FourPos[row][col][2];
    while (scelta!=10) {
      lcd.clear();

      lcd.print("CH+=OK CH-=Exit"); 
      lcd.setCursor(0,1);
      lcd.print("x=
   y=    z=   ");
      lcd.setCursor(2,1);
      lcd.print(calx);
      lcd.setCursor(7,1);

      lcd.print(caly);
      lcd.setCursor(13,1);
      lcd.print(calz);

      scelta=Read_RC();    
      switch(scelta) {
        case 1: calx--;
  break;
        case 3: calx++; break;
        case 4: caly--; break;
        case
  6: caly++; break;
        case 7: calz--; break;
        case 9: calz++; break;

        case 11:
          FourPos[row][col][0] = calx;
          FourPos[row][col][1]
  = caly;
          FourPos[row][col][2] = calz;
          Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),calx,caly,calz,90+degrees(atan2(-calx,abs(caly))));

          break;
      }
    }
  }  
}  

void ShowDefeat() {

  Set_Arm(0,130,100);
  for(int k=0; k<=180*2; k++) {
    MyServoWrite(SERVO6,
  abs(180*sin(radians(k)))); 
    delay(10);}
  }

void ShowWin() {

  Set_Arm(0,100,300);
  for(int k=0; k<=180*3; k ++){
    MyServoWrite(SERVO6,
  abs(180*sin(radians(k)))); 
    delay(4);}}

Downloadable files

SCHEMATICS

Follow the above wiring diagram to wire Arduino

SCHEMATICS

Follow the above wiring diagram to wire Arduino

SCHEMATICS

Follow the above wiring diagram to wire Arduino

SCHEMATICS

Comments

Only logged in users can leave comments

dfilannino03

0 Followers

•

0 Projects

Intro