Components and supplies
Jumper wires (generic)
Arduino UNO
5V 2.5A Switching Power Supply
SG90 Micro-servo motor
Tools and machines
Drill / Driver, Cordless
Multitool, Screwdriver
Project description
Code
Cube_Solver_No_GUI_Koc_chen.ino
arduino
1 2 3/* 4 * Developed and integrated by: Hernando Bolanos 5 * 6 * code lines and sOurces: 7 * 1.Sweep by BARRAGAN <http://barraganstudio.com> 8 * modified 8 Nov 2013 9 * 2. by Scott Fitzgerald 10 * http://www.arduino.cc/en/Tutorial/Sweep 11 12 13* 3. Instructables -Matt2Yu -ALGORTIHM, CONCEPT FOR CFOP METHOD 14* Version 1- 16-AGOSTO-2020 15* 16 17* Best calibration settings: -move speed 6-push 146- take into account the servos orientation- if it is not the same you have to adapt all the calibration angles and timing 18* Is must to use PWM outputs- This was implmented with a nodemcu 19*/ 20 21 22#include <Servo.h> 23//#include <Solver.h> 24 25// servo objects 26Servo rotate_servo; 27Servo push_servo; 28 29int move_speed = 6 ; //6 - CALIBRATION -PUSH 146 30int buffer_time = 105; // time between moves 31int rotate_pos = 90; 32int push_pos = 110; 33int hold_progress = 3; 34int offset_degrees = 3; 35bool slow_push = false; 36String kociemba_sol = ""; 37 38//////// cube move variables: 39bool sim_only = false; 40 41// test function: 42bool test_ongoing = true; 43 44 45 46 47////////////////////// Serial Communication (receive arrays or kociemba solution from any .py script) 48 49 50void accept_string() 51{ 52 53 char ready_signal = 'ready'; 54 char received_signal = 'received'; 55 56 for (int piece_num = 0; piece_num <5; piece_num++) 57 { 58 // send ready signal 59 Serial.println(ready_signal); 60 delay(100); 61 } 62 // receive string 63 while(kociemba_sol == "") 64 { 65 char character; 66 while(Serial.available()) 67 { 68 character = Serial.read(); 69 kociemba_sol.concat(character); 70 } 71 } 72 73 74 75 delay(10); 76 Serial.print("String Aceptado: "); 77 Serial.print(kociemba_sol); 78 79 // send color confirmed signal 80 Serial.println("arduino dice:"); 81 Serial.println(received_signal); 82 Serial.println(kociemba_sol); 83 delay(10); 84} 85 86 87 88 89///////// Cube movement functions: //////////// 90 91int move_servo(int start, int finish, int servo_pin) 92{ 93 int pos; 94 if (start - finish < 0) 95 { 96 for(pos = start; pos <= finish; pos += 1) 97 { 98 if (servo_pin == 6) 99 { 100 push_servo.write(pos); 101 102 delay(move_speed); 103 } 104 else if (servo_pin == 9) 105 { 106 rotate_servo.write(pos); 107 delay(move_speed); 108 } 109 } 110 } 111 else 112 { 113 for(pos = start; pos >= finish; pos -= 1) 114 { 115 if (servo_pin == 6) 116 { 117 push_servo.write(pos); 118 119 delay(move_speed); 120 } 121 else if (servo_pin == 9) 122 { 123 rotate_servo.write(pos); 124 delay(move_speed); 125 } 126 } 127 } 128 // use a swich case next time 129 if (servo_pin == 9) 130 { 131 rotate_pos = pos; 132 } 133 if (servo_pin == 6) 134 { 135 push_pos = pos; 136 } 137 delay(buffer_time); 138} 139///////// Cube movement functions: //////////// 140void push_cube(int num_of_pushes = 1) 141{ 142 if (num_of_pushes == 1) 143 { 144 if (slow_push == false) 145 { 146 move_servo(push_pos, 146, 6);//72 147 delay(buffer_time); 148 release_cube(); 149 delay(buffer_time); 150 } 151 else // on rotate one 152 { 153 move_servo(push_pos, 146, 6);//72 154 delay(buffer_time+200); 155 release_cube(); 156 delay(buffer_time); 157 } 158 } 159 else 160 { 161 while (num_of_pushes != 0) 162 { 163 if (slow_push == false) 164 { 165 move_servo(push_pos, 146, 6);//72 166 delay(buffer_time+50); 167 move_servo(push_pos, 110, 6);//original 120 168 delay(buffer_time); 169 num_of_pushes--; 170 } 171 else // on rotate one 172 { 173 move_servo(push_pos, 110, 6);//72 174 delay(buffer_time+200); 175 move_servo(push_pos, 146, 6);//original 130 176 delay(buffer_time); 177 num_of_pushes--; 178 } 179 } 180 release_cube(); 181 } 182} 183void hold_cube() 184{ 185 move_servo(push_pos, 133, 6);// 186 hold_progress = 1; 187} 188void release_cube() 189{ 190 move_servo(push_pos, 105, 6); 191 hold_progress = 3; 192} 193void rotate_one() 194{ 195 slow_push = true; 196 int rotate_finish = -5;//11-4 197 if (hold_progress == 1) // hold progress 1 = hold 198 { 199 // from rotate_two 200 if (rotate_pos < 140) 201 { 202 // initial turn 203 move_servo(rotate_pos, rotate_finish-2, 9); 204 move_servo(rotate_pos, rotate_finish+5, 9); 205 // release and turn some more 206 release_cube(); 207 move_servo(rotate_pos, 101, 9); 208 hold_cube(); 209 move_servo(rotate_pos, 75, 9);//82original/72 /75 210 move_servo(rotate_pos, 92, 9); // prevent pulling 211 release_cube(); 212 move_servo(rotate_pos, rotate_finish, 9); 213 } 214 215 // from rotate_three 216 else if (rotate_pos > 140) 217 { 218 // initial turn 219 move_servo(rotate_pos, rotate_finish-5, 9); 220 move_servo(rotate_pos, rotate_finish+10, 9); 221 // release and turn some more 222 release_cube(); 223 move_servo(rotate_pos, 108, 9); 224 hold_cube(); 225 move_servo(rotate_pos, 83, 9);//83 origial 226 move_servo(rotate_pos, 93, 9); // prevent pulling 227 release_cube(); 228 move_servo(rotate_pos, rotate_finish, 9); 229 } 230 231 232 233 234 hold_progress = 2; 235 } 236 else if (hold_progress == 2) // hold progress 2 = release, but offset still there 237 { 238 hold_progress = 3; 239 move_servo(rotate_pos, rotate_finish, 9); 240 } 241 else if (hold_progress == 3) // hold progress 3 = release, offsets reconciled 242 { 243 // do nothing 244 move_servo(rotate_pos, rotate_finish, 9); 245 } 246} 247void rotate_two() 248{ 249 slow_push = false; 250 int rotate_finish = 90; 251 if (hold_progress == 1) // hold progress 1 = hold 252 { 253 // rotate from rotate_one 254 if (rotate_pos < 50) 255 { 256 // initial turn 257 move_servo(rotate_pos, rotate_finish+3, 9); 258 move_servo(rotate_pos, rotate_finish-5, 9); 259 260 // release and turn some more 261 262 release_cube(); 263 move_servo(rotate_pos, 0, 9); 264 hold_cube(); 265 move_servo(rotate_pos, 18, 9);//original 18/25/20 266 move_servo(rotate_pos, 12, 9); // prevent pulling/original 8/2 267 release_cube(); 268 269 move_servo(rotate_pos, rotate_finish, 9); 270 } 271 // rotate from rotate_three 272 else if (rotate_pos > 150) 273 { 274 move_servo(rotate_pos, rotate_finish-1, 9); 275 move_servo(rotate_pos, rotate_finish+1, 9); 276 277 278 // release and turn some more 279 release_cube(); 280 move_servo(rotate_pos, 179, 9); 281 hold_cube(); 282 move_servo(rotate_pos, 170, 9);//orignal 170 283 move_servo(rotate_pos, 178, 9); // prevent pulling 284 release_cube(); 285 move_servo(rotate_pos, rotate_finish, 9); 286 } 287 hold_progress = 2; 288 } 289 else if (hold_progress == 2) // hold progress 2 = release, but offset still there 290 { 291 hold_progress = 3; 292 move_servo(rotate_pos, rotate_finish, 9); 293 } 294 else if (hold_progress == 3) // hold progress 3 = release, offsets reconciled 295 { 296 // do nothing 297 move_servo(rotate_pos, rotate_finish, 9); 298 } 299} 300void rotate_three() 301{ 302 slow_push = false; 303 int rotate_finish = 179; 304 if (hold_progress == 1) // hold progress 1 = hold 305 { 306 // from rotate_two 307 if (rotate_pos > 40) 308 { 309 move_servo(rotate_pos, rotate_finish+10, 9); 310 move_servo(rotate_pos, rotate_finish-1, 9); // prevent pulling 311 312 // fix: cube not fully turned 313 release_cube(); 314 move_servo(rotate_pos, 90, 9); 315 hold_cube(); 316 move_servo(rotate_pos, 105, 9);// original 110 317 move_servo(rotate_pos, 106, 9); // prevent pulling-100 318 release_cube(); 319 move_servo(rotate_pos, rotate_finish, 9); 320 } 321 322 // from rotate_one 323 if (rotate_pos < 40) 324 { 325 move_servo(rotate_pos, rotate_finish+5, 9); 326 move_servo(rotate_pos, rotate_finish-3, 9); // prevent pulling 327 328 // fix: cube not fully turned 329 release_cube(); 330 move_servo(rotate_pos, 80, 9); 331 hold_cube(); 332 move_servo(rotate_pos, 110, 9);//original 100/110 333 move_servo(rotate_pos, 90, 9); // prevent pulling 334 release_cube(); 335 move_servo(rotate_pos, rotate_finish, 9); 336 } 337 338 hold_progress = 2; 339 } 340 else if (hold_progress == 2) // hold progress 2 = release, but offset still there 341 { 342 hold_progress = 3; 343 move_servo(rotate_pos, rotate_finish, 9); 344 } 345 else if (hold_progress == 3) // hold progress 3 = release, offsets reconciled 346 { 347 // do nothing 348 move_servo(rotate_pos, rotate_finish, 9); 349 } 350} 351 352 353///////////////////// Cube Move Notation /////////////////////////// 354// They print, simulate and call the physical functions 355 356void right_inverted() 357{ 358 Serial.println("R', "); 359 360 if (sim_only == false) 361 { 362 rotate_three(); 363 push_cube(); 364 hold_cube(); 365 rotate_two(); 366 release_cube(); 367 rotate_one(); 368 push_cube(); 369 rotate_two(); 370 push_cube(3); 371 } 372 373} 374void right() 375{ 376 Serial.println("R, "); 377 378 if (sim_only == false) 379 { 380 rotate_three(); 381 push_cube(); 382 rotate_two(); 383 hold_cube(); 384 rotate_three(); 385 release_cube(); 386 rotate_one(); 387 push_cube(); 388 rotate_two(); 389 push_cube(); 390 } 391 } 392void left_inverted() 393{ 394 Serial.println("L', "); 395 396 if (sim_only == false) 397 { 398 rotate_one(); 399 push_cube(); 400 rotate_two(); 401 hold_cube(); 402 rotate_one(); 403 release_cube(); 404 rotate_three(); 405 push_cube(); 406 rotate_two(); 407 push_cube(); 408 } 409 410} 411void left() 412{ 413 Serial.println("L, "); 414 415 if(sim_only == false) 416 { 417 rotate_one(); 418 push_cube(); 419 hold_cube(); 420 rotate_two(); 421 release_cube(); 422 rotate_three(); 423 push_cube(); 424 rotate_two(); 425 push_cube(3); 426 } 427 428} 429void down_inverted() 430{ 431 Serial.println("D', "); 432 433 if (sim_only == false) 434 { 435 hold_cube(); 436 rotate_one(); 437 release_cube(); 438 rotate_two(); 439 push_cube(); 440 rotate_one(); 441 push_cube(); 442 rotate_two(); 443 push_cube(3); 444 } 445 446} 447void down() 448{ 449 Serial.println("D, "); 450 451 if (sim_only == false) 452 { 453 hold_cube(); 454 rotate_three(); 455 release_cube(); 456 rotate_two(); 457 push_cube(); 458 rotate_three(); 459 push_cube(); 460 rotate_two(); 461 push_cube(3); 462 } 463 464} 465void up_inverted() 466{ 467 Serial.println("U', "); 468 469 if (sim_only == false) 470 { 471 push_cube(2); 472 hold_cube(); 473 rotate_one(); 474 release_cube(); 475 rotate_two(); 476 push_cube(); 477 rotate_one(); 478 push_cube(); 479 rotate_two(); 480 push_cube(); 481 } 482 483} 484void up() 485{ 486 Serial.println("U, "); 487 488 if (sim_only == false) 489 { 490 push_cube(2); 491 hold_cube(); 492 rotate_three(); 493 release_cube(); 494 rotate_two(); 495 push_cube(); 496 rotate_three(); 497 push_cube(); 498 rotate_two(); 499 push_cube(); 500 } 501 502} 503void front_inverted() 504{ 505 Serial.println("F', "); 506 507 if (sim_only == false) 508 { 509 push_cube(3); 510 hold_cube(); 511 rotate_one(); 512 release_cube(); 513 rotate_two(); 514 push_cube(); 515 rotate_one(); 516 push_cube(); 517 rotate_two(); 518 } 519 520} 521void front() 522{ 523 Serial.println("F, "); 524 525 if (sim_only == false) 526 { 527 push_cube(3); 528 hold_cube(); 529 rotate_three(); 530 release_cube(); 531 rotate_two(); 532 push_cube(); 533 rotate_three(); 534 push_cube(); 535 rotate_two(); 536 } 537} 538void back_inverted() 539{ 540 Serial.println("B', "); 541 if (sim_only == false) 542 { 543 push_cube(); 544 hold_cube(); 545 rotate_one(); // ccw 546 release_cube(); 547 rotate_two(); 548 push_cube(3); 549 rotate_three(); //cw 550 push_cube(); 551 rotate_two(); 552 } 553 554} 555void back() 556{ 557 Serial.println("B, "); 558 559 if (sim_only == false) 560 { 561 push_cube(); 562 hold_cube(); 563 rotate_three(); 564 release_cube(); 565 rotate_two(); 566 push_cube(3); 567 rotate_one(); 568 push_cube(); 569 rotate_two(); 570 } 571 572} 573 574// insert top layer edges 575 576// miscellaneous algorithms 577void warm_up() // do it six times to get back to the original position 578{ 579 Serial.println(); 580 Serial.print("Warmup: "); 581 Serial.print("R', D', R, D"); 582 //r' 583 rotate_one(); 584 push_cube(); 585 hold_cube();// 586 rotate_two(); 587 release_cube(); 588 rotate_three(); 589 push_cube(); 590 rotate_two(); 591 push_cube(3); 592 593 //d' 594 595 hold_cube();// 596 rotate_three(); 597 release_cube(); 598 599 //r start here 600 601 rotate_two(); 602 push_cube(); 603 rotate_three(); 604 hold_cube(); 605 rotate_two(); 606 release_cube(); 607 608 // d 609 610 rotate_three(); 611 push_cube(); 612 hold_cube(); 613 rotate_two(); 614 release_cube(); 615 push_cube(); 616 rotate_one(); 617 push_cube(); 618 rotate_two(); 619 push_cube(3); 620} 621void superflip() // all edges are opposite (checkered pattern) 622{ 623 Serial.println(); 624 Serial.println("Superflip: "); 625 up(); 626 up(); 627 down(); 628 down(); 629 630 left(); 631 left(); 632 right(); 633 right(); 634 635 636 front(); 637 front(); 638 back(); 639 back(); 640} 641 642 // test it 643void scramble() // random 25 moves - if you need continuos solve-scramble and resolution this part of code would be usefull 644{ 645 Serial.println(); 646 Serial.println("Scramble: "); 647 int move; 648 for(int j = 0; j < 25; j++) 649 { 650 move = random(1, 12); 651 //Serial.println(move); 652 switch(move) 653 { 654 case 1: 655 right(); 656 break; 657 case 2: 658 right_inverted(); 659 break; 660 case 3: 661 left(); 662 break; 663 case 4: 664 left_inverted(); 665 break; 666 case 5: 667 up(); 668 break; 669 case 6: 670 up_inverted(); 671 break; 672 case 7: 673 down(); 674 break; 675 case 8: 676 down_inverted(); 677 break; 678 case 9: 679 front(); 680 break; 681 case 10: 682 front_inverted(); 683 break; 684 case 11: 685 back(); 686 break; 687 case 12: 688 back_inverted(); 689 break; 690 } 691 } 692} 693 694 695 696 697void run_kociemba(){ 698 699//"L2 D' R' L2 D2 R F' D2 F2 R' B' L2 D R2 L2 D L2 B2 U' B2 U2" 700 701 702kociemba_sol= "B L' F' B R F2 U2 D R2 F D F2 U R2 F2 L2 B2 R2 D2 R2"; 703//"L U' L B2 U2 F' D R2 F L F2 R2 F2 U2 D B2 L2 D2 B2 U'"; 704 705// Length (with one extra character for the null terminator) 706int str_len = kociemba_sol.length() + 1; 707 708 709//Serial.println("arduino : STRINGS RECEIVED:"); 710//Serial.println((str_len-1)); 711 712for(int i = 0; i <= (str_len-1); i++){ //recorre 713 714//Serial.print(i); 715 716 717if ((kociemba_sol.charAt(i)) =='R'){ 718 719 720if ((kociemba_sol.charAt(i+1)) == '2') { 721 right(); 722 right();} 723 724 725if ((kociemba_sol.charAt(i+1)) == '\\'') { 726 right_inverted();} 727 728if (((kociemba_sol.charAt(i+1))!= '2') and((kociemba_sol.charAt(i+1)) != '\\'')) { 729 right();} 730 731} 732 733else { // DO NOTHING 734 735} 736 737if ((kociemba_sol.charAt(i)) =='L'){ 738 739 740if ((kociemba_sol.charAt(i+1)) == '2') { 741 left(); 742 left();} 743 744 745if ((kociemba_sol.charAt(i+1)) == '\\'') { 746 left_inverted();} 747 748if (((kociemba_sol.charAt(i+1))!= '2') and((kociemba_sol.charAt(i+1)) != '\\'')) { 749 left();} 750 751} 752 753else { //DO NOTHING 754 755} 756 757if ((kociemba_sol.charAt(i)) =='U'){ 758 759 760if ((kociemba_sol.charAt(i+1)) == '2') { 761 up(); 762 up();} 763 764 765if ((kociemba_sol.charAt(i+1)) == '\\'') { 766 up_inverted();} 767 768if (((kociemba_sol.charAt(i+1))!= '2') and((kociemba_sol.charAt(i+1)) != '\\'')) { 769 up();} 770 771} 772 773else { //DO NOTHING 774 775} 776 777 778 779if ((kociemba_sol.charAt(i)) =='D'){ 780 781 782if ((kociemba_sol.charAt(i+1)) == '2') { 783 down(); 784 down();} 785 786 787if ((kociemba_sol.charAt(i+1)) == '\\'') { 788 down_inverted();} 789 790if (((kociemba_sol.charAt(i+1))!= '2') and((kociemba_sol.charAt(i+1)) != '\\'')) { 791 down();} 792 793} 794 795else { //DO NOTHNIG 796 797} 798 799 800 801 802 803if ((kociemba_sol.charAt(i)) =='F'){ 804 805 806if ((kociemba_sol.charAt(i+1)) == '2') { 807 front(); 808 front();} 809 810 811if ((kociemba_sol.charAt(i+1)) == '\\'') { 812 front_inverted();} 813 814if (((kociemba_sol.charAt(i+1))!= '2') and((kociemba_sol.charAt(i+1)) != '\\'')) { 815 front();} 816 817} 818 819else { //DO NOTHING 820 821} 822 823 824 825 826if ((kociemba_sol.charAt(i)) =='B'){ 827 828 829if ((kociemba_sol.charAt(i+1)) == '2') { 830 back(); 831 back();} 832 833 834if ((kociemba_sol.charAt(i+1)) == '\\'') { 835 back_inverted();} 836 837if (((kociemba_sol.charAt(i+1))!= '2') and((kociemba_sol.charAt(i+1)) != '\\'')) { 838 back();} 839 840} 841 842else { //DO NOTHING 843 844} 845 846} 847 848 849 //warm_up(); 850 //scramble(); 851} 852 853void show_off_cube() 854{ 855 rotate_one(); 856 rotate_three(); 857 push_cube(2); 858 rotate_one(); 859} 860//////////////////////////////////////////////////////////////////////////////////////////////////// 861////////////////////////////////////// PROGRAM START /////////////////////////////////////////////// 862//////////////////////////////////////////////////////////////////////////////////////////////////// 863 864void setup() 865{ 866 rotate_servo.attach(12); // attaches the servo on pin 9 to the servo object 867 push_servo.attach(14); // attaches the servo on pin 6 to the servo object 868 push_servo.write(push_pos); 869 rotate_servo.write(rotate_pos); 870 delay(1000); 871 Serial.begin(9600); 872 while (! Serial); 873 874} 875 876/////////////// Se recibe por puerto serial la solucion ////////////////// 877 878void loop() 879{ 880//activate accept string when you want to receive solution from PY script 881 882 883//accept_string(); 884 885 886 //Warming up 887 888Serial.println("Warming Up: will back to the original position and start "); 889 890 891push_cube(); 892push_cube(); 893push_cube(); 894push_cube(); 895 896delay(2000); 897 898 899Serial.println("Arduino dice:Inicia a correr la solucion:"); 900 901 902 run_kociemba(); //corre el string recibido 903 904Serial.println("Arduino : Finish-check all the cube face solved"); 905 906//show all the cube solved 907 908show_off_cube(); 909 910 911Serial.println("Arduino :Finish- send a new kocoemba solution"); 912 913 914 while(true){} 915 916 917 918} 919
Downloadable files
Schematics
Schematics-servo + arduino- source:hac4kernaren
Schematics
Schematics
Schematics-servo + arduino- source:hac4kernaren
Schematics
Comments
Only logged in users can leave comments
isramodd
2 years ago
How you be able to add the solution algorithm to the arduino code, withou using a camera??
Anonymous user
2 years ago
Can you share code with the camera one
hbolanos2001
2 years ago
Hello Are still looking for the software?
Purab_19
4 years ago
how did you put colour in arduino
Purab_19
4 years ago
Can you share code with the camera one
hbolanos2001
2 years ago
Hello Are still looking for the software?
ozbarda
a year ago
The robot is not flipping the cube why might this be happening?