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
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
Downloadable files
SCHEMATICS
Follow the above wiring diagram to wire Arduino
SCHEMATICS
SCHEMATICS
Follow the above wiring diagram to wire Arduino
SCHEMATICS
SCHEMATICS
Follow the above wiring diagram to wire Arduino
SCHEMATICS
Comments
Only logged in users can leave comments
dfilannino03
0 Followers
•0 Projects
Table of contents
Intro
8
0