Rubik's Cube Solver Robot

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

1
2
3Instructables -Matt2Yu
4Version 0- 15-mayo-2020
5
6*/
7
8
9#include
10  <Servo.h>
11//#include <Solver.h>
12 
13// servo objects
14Servo rotate_servo;
15Servo
16  push_servo;
17 
18int move_speed = 8 ; 
19int buffer_time = 115; // time between
20  moves
21int rotate_pos = 90;
22int push_pos = 180;
23int hold_progress = 3;
24int
25  offset_degrees = 10;
26bool slow_push = false;
27String kociemba_sol = "";
28
29  
30//////// cube move variables:
31bool sim_only = false;
32 
33// test function:
34bool
35  test_ongoing = true;
36 
37                    
38
39 
40//////////////////////
41  Serial Communication (receive arrays from .py script)
42 
43 
44void accept_string()
45{
46
47        
48        char ready_signal = 'ready';
49        char received_signal
50  = 'received';
51 
52        for (int piece_num = 0; piece_num <5; piece_num++)
53
54        {       
55                // send ready signal
56                Serial.println(ready_signal);
57
58                delay(100);
59        }
60        // receive string
61        while(kociemba_sol
62  == "")
63        {
64                char character;
65                while(Serial.available())
66
67                {
68                    character = Serial.read();
69                        kociemba_sol.concat(character);
70
71                }
72        }
73        delay(10);
74        Serial.print("String
75  Aceptado: ");
76       Serial.print(kociemba_sol);
77 
78        // send color
79  confirmed signal
80        Serial.println("arduino dice:");
81        Serial.println(received_signal);
82
83        Serial.println(kociemba_sol);
84        delay(10);
85}
86 
87
88/////////////////////
89  Physical Movement Functions ///////////////////////////
90 
91////////// Servo
92  movement function: ///////////
93int move_servo(int start, int finish, int servo_pin)
94{
95
96        int pos;
97        if (start - finish < 0)
98        {
99                for(pos
100  = start; pos <= finish; pos += 1)
101                {
102                        if
103  (servo_pin == 6)
104                        {
105                                push_servo.write(pos);
106
107                                delay(move_speed);
108                        }
109
110                        else if (servo_pin == 9)
111                        {
112
113                                rotate_servo.write(pos);
114                                delay(move_speed);
115
116                        }
117                }
118        }
119        else
120
121        {
122                for(pos = start; pos >= finish; pos -= 1)
123                {
124
125                        if (servo_pin == 6)
126                        {
127                                push_servo.write(pos);
128
129                                delay(move_speed);
130                        }
131
132                        else if (servo_pin == 9)
133                        {
134
135                                rotate_servo.write(pos);
136                                delay(move_speed);
137
138                        }
139                }
140        }
141        // use a
142  swich case next time
143        if (servo_pin == 9)
144        {
145                rotate_pos
146  = pos;
147        }
148        if (servo_pin == 6)
149        {
150                push_pos
151  = pos;
152        }
153        delay(buffer_time);
154}
155///////// Cube movement
156  functions: ////////////
157
158void push_cube(int num_of_pushes = 1)
159{
160        if
161  (num_of_pushes == 1)
162                {
163                        if (slow_push
164  == false)
165                        {
166                                move_servo(push_pos,
167  98, 6);
168                                delay(buffer_time);
169                                release_cube();
170
171                                delay(buffer_time);
172                        }
173
174                        else // on rotate one
175                        {
176                                move_servo(push_pos,
177  98, 6);
178                                delay(buffer_time+200);
179                                release_cube();
180
181                                delay(buffer_time);
182                        }
183
184                }
185        else
186        {
187                while (num_of_pushes
188  != 0)
189                {
190                        if (slow_push == false)
191
192                        {
193                                move_servo(push_pos,
194  98, 6);
195                                delay(buffer_time+50);
196                                move_servo(push_pos,
197  160,6);
198                                delay(buffer_time);
199                                num_of_pushes--;
200
201                        }
202                        else // on rotate one
203                        {
204
205                                move_servo(push_pos, 98, 6);
206                                delay(buffer_time+200);
207
208                                move_servo(push_pos, 160, 6);
209                                delay(buffer_time);
210
211                                num_of_pushes--;
212                        }      
213
214                }
215        release_cube();
216        }
217}
218void hold_cube()
219{
220
221        move_servo(push_pos, 140, 6);
222        hold_progress = 1;
223}
224void
225  release_cube()
226{
227        move_servo(push_pos, 180, 6);
228        hold_progress
229  = 3;
230}
231void rotate_one()
232{
233        slow_push = true;
234        int
235  rotate_finish = 0;
236        if (hold_progress == 1) // hold progress 1 = hold
237
238        {
239                // from rotate_two
240                if (rotate_pos
241  < 140)
242                {
243                        // initial turn
244                        move_servo(rotate_pos,
245  rotate_finish-9, 9);
246                        move_servo(rotate_pos, rotate_finish+10,
247  9);
248                        // release and turn some more
249                        release_cube();
250
251                        move_servo(rotate_pos, 101, 9);
252                        hold_cube();
253
254                        move_servo(rotate_pos, 64, 9);
255                        move_servo(rotate_pos,
256  92, 9); // prevent pulling
257                        release_cube();
258                        move_servo(rotate_pos,
259  rotate_finish, 9);
260                }
261 
262                // from rotate_three
263
264                else if (rotate_pos > 140)
265                {
266                        //
267  initial turn
268                        move_servo(rotate_pos, rotate_finish-3,
269  9);
270                        move_servo(rotate_pos, rotate_finish+3, 9);
271                        //
272  release and turn some more
273                        release_cube();
274                        move_servo(rotate_pos,
275  115, 9);
276                        hold_cube();
277                        move_servo(rotate_pos,
278  64, 9);
279                        move_servo(rotate_pos, 93, 9); // prevent pulling
280
281                        release_cube();
282                        move_servo(rotate_pos,
283  rotate_finish, 9);
284                }
285 
286 
287 
288 
289                hold_progress
290  = 2;
291        }
292        else if (hold_progress == 2) // hold progress 2 = release,
293  but offset still there
294        {
295                hold_progress = 3;
296                move_servo(rotate_pos,
297  rotate_finish, 9);
298        }
299        else if (hold_progress == 3) // hold
300  progress 3 = release, offsets reconciled
301        {
302                // do nothing
303
304                move_servo(rotate_pos, rotate_finish, 9);
305        }
306}
307void
308  rotate_two()
309{
310        slow_push = false;
311        int rotate_finish = 90;
312
313        if (hold_progress == 1) // hold progress 1 = hold
314        {
315                //
316  rotate from rotate_one
317                if (rotate_pos < 50)
318                {
319
320                        // initial turn
321                        move_servo(rotate_pos,
322  rotate_finish+10, 9);
323                        move_servo(rotate_pos, rotate_finish-5,
324  9);
325 
326                        // release and turn some more
327                       
328
329                        release_cube();
330                        move_servo(rotate_pos,
331  0, 9);
332                        hold_cube();
333                        move_servo(rotate_pos,
334  8, 9);
335                        move_servo(rotate_pos, 5, 9); // prevent pulling
336
337                        release_cube();
338                       
339                        move_servo(rotate_pos,
340  rotate_finish, 9);
341                }
342                // rotate from rotate_three
343
344                else if (rotate_pos > 150)
345                {
346                        move_servo(rotate_pos,
347  rotate_finish-12, 9);
348                        move_servo(rotate_pos, rotate_finish+4,
349  9);
350 
351 
352                        // release and turn some more
353                        release_cube();
354
355                        move_servo(rotate_pos, 180, 9);
356                        hold_cube();
357
358                        move_servo(rotate_pos, 170, 9);
359                        move_servo(rotate_pos,
360  178, 9); // prevent pulling
361                        release_cube();
362                        move_servo(rotate_pos,
363  rotate_finish, 9);
364                }
365                hold_progress = 2;
366
367        }
368        else if (hold_progress == 2) // hold progress 2 = release,
369  but offset still there
370        {
371                hold_progress = 3;
372                move_servo(rotate_pos,
373  rotate_finish, 9);
374        }
375        else if (hold_progress == 3) // hold
376  progress 3 = release, offsets reconciled
377        {
378                // do nothing
379
380                move_servo(rotate_pos, rotate_finish, 9);
381        }
382}
383void
384  rotate_three()
385{
386        slow_push = false;
387        int rotate_finish =
388  180;
389        if (hold_progress == 1) // hold progress 1 = hold
390        {
391
392                // from rotate_two
393                if (rotate_pos > 40)
394                {
395
396                        move_servo(rotate_pos, rotate_finish+4, 9);
397                        move_servo(rotate_pos,
398  rotate_finish-5, 9); // prevent pulling
399 
400                        // fix:
401  cube not fully turned
402                        release_cube();
403                        move_servo(rotate_pos,
404  80, 9);
405                        hold_cube();
406                        move_servo(rotate_pos,
407  104, 9);
408                        move_servo(rotate_pos, 90, 9); // prevent pulling
409
410                        release_cube();
411                        move_servo(rotate_pos,
412  rotate_finish, 9);
413                }
414 
415                // from rotate_one
416
417                if (rotate_pos < 40)
418                {
419                        move_servo(rotate_pos,
420  rotate_finish+5, 9);
421                        move_servo(rotate_pos, rotate_finish-7,
422  9); // prevent pulling
423 
424                        // fix: cube not fully turned
425
426                        release_cube();
427                        move_servo(rotate_pos,
428  70, 9);
429                        hold_cube();
430                        move_servo(rotate_pos,
431  85, 9);
432                        move_servo(rotate_pos,105, 9); // prevent pulling
433
434                        release_cube();
435                        move_servo(rotate_pos,
436  rotate_finish, 9);
437                }
438 
439                hold_progress =
440  2;
441        }
442        else if (hold_progress == 2) // hold progress 2 = release,
443  but offset still there
444        {
445                hold_progress = 3;
446                move_servo(rotate_pos,
447  rotate_finish, 9);
448        }
449        else if (hold_progress == 3) // hold
450  progress 3 = release, offsets reconciled
451        {
452                // do nothing
453
454                move_servo(rotate_pos, rotate_finish, 9);
455        }
456}
457
458  
459///////////////////// Cube Move Notation ///////////////////////////
460// They
461  print, simulate and call the physical functions
462 
463void right_inverted()
464{
465
466        Serial.println("R', ");
467 
468        if (sim_only == false)
469        {
470
471                rotate_three();
472                push_cube();
473                hold_cube();
474
475                rotate_two();
476                release_cube();
477                rotate_one();
478
479                push_cube();
480                rotate_two();
481                push_cube(3);
482
483        }
484 
485}
486void right()
487{
488        Serial.println("R, ");
489
490  
491        if (sim_only == false)
492        {
493                rotate_three();
494
495                push_cube();
496                rotate_two();
497                hold_cube();
498
499                rotate_three();
500                release_cube();
501                rotate_one();
502
503                push_cube();
504                rotate_two();
505                push_cube();
506
507        }
508 }
509void left_inverted()
510{
511        Serial.println("L', ");
512
513  
514        if (sim_only == false)
515        {
516                rotate_one();
517
518                push_cube();
519                rotate_two();
520                hold_cube();
521
522                rotate_one();
523                release_cube();
524                rotate_three();
525
526                push_cube();
527                rotate_two();
528                push_cube();
529
530        }
531 
532}
533void left()
534{
535        Serial.println("L, ");
536
537  
538        if(sim_only == false)
539        {
540                rotate_one();
541
542                push_cube();
543                hold_cube();
544                rotate_two();
545
546                release_cube();
547                rotate_three();
548                push_cube();
549
550                rotate_two();
551                push_cube(3);
552        }
553
554  
555}
556void down_inverted()
557{
558        Serial.println("D', ");
559 
560
561        if (sim_only == false)
562        {
563                hold_cube();
564
565                rotate_one();
566                release_cube();
567                rotate_two();
568
569                push_cube();
570                rotate_one();
571                push_cube();
572
573                rotate_two();
574                push_cube(3);
575        }
576
577  
578}
579void down()
580{
581        Serial.println("D, ");
582 
583        if
584  (sim_only == false)
585        {
586                hold_cube();
587                rotate_three();
588
589                release_cube();
590                rotate_two();
591                push_cube();
592
593                rotate_three();
594                push_cube();
595                rotate_two();
596
597                push_cube(3);
598        }
599 
600}
601void up_inverted()
602{
603
604        Serial.println("U', ");
605 
606        if (sim_only == false)
607        {
608
609                push_cube(2);
610                hold_cube();
611                rotate_one();
612
613                release_cube();
614                rotate_two();
615                push_cube();
616
617                rotate_one();
618                push_cube();
619                rotate_two();
620
621                push_cube();
622        }
623 
624}
625void up()
626{
627        Serial.println("U,
628  ");
629 
630        if (sim_only == false)
631        {
632                push_cube(2);
633
634                hold_cube();
635                rotate_three();
636                release_cube();
637
638                rotate_two();
639                push_cube();
640                rotate_three();
641
642                push_cube();
643                rotate_two();
644                push_cube();
645
646        }
647 
648}
649void front_inverted()
650{
651        Serial.println("F',
652  ");
653 
654        if (sim_only == false)
655        {
656        push_cube(3);
657
658        hold_cube();
659        rotate_one();
660        release_cube();
661        rotate_two();
662
663        push_cube();
664        rotate_one();
665        push_cube();
666        rotate_two();
667
668        }
669 
670}
671void front()
672{
673        Serial.println("F, ");
674
675  
676        if (sim_only == false)
677        {
678                push_cube(3);
679
680                hold_cube();
681                rotate_three();
682                release_cube();
683
684                rotate_two();
685                push_cube();
686                rotate_three();
687
688                push_cube();
689                rotate_two();
690        }
691}
692void
693  back_inverted()
694{
695        Serial.println("B', ");
696        if (sim_only
697  == false)
698        {
699                push_cube();
700                hold_cube();
701
702                rotate_one(); // ccw
703                release_cube();
704                rotate_two();
705
706                push_cube(3);
707                rotate_three(); //cw
708                push_cube();
709
710                rotate_two();
711        }
712 
713}
714void back()
715{
716        Serial.println("B,
717  ");
718 
719        if (sim_only == false)
720        {
721                push_cube();
722
723                hold_cube();
724                rotate_three();
725                release_cube();
726
727                rotate_two();
728                push_cube(3);
729                rotate_one();
730
731                push_cube();
732                rotate_two();
733        }
734 
735}
736    
737
738// insert top layer edges
739
740// miscellaneous algorithms
741void
742  warm_up() // do it six times to get back to the original position
743{
744        Serial.println();
745
746        Serial.print("Warmup: ");
747        Serial.print("R', D', R, D");
748
749        //r'
750        rotate_one();
751        push_cube();
752        hold_cube();//
753
754        rotate_two();
755        release_cube();
756        rotate_three();
757
758        push_cube();
759        rotate_two();
760        push_cube(3);
761 
762
763        //d'
764 
765        hold_cube();//
766        rotate_three();
767        release_cube();
768
769  
770        //r start here
771 
772        rotate_two();
773        push_cube();
774
775        rotate_three();
776        hold_cube();
777        rotate_two();
778        release_cube();
779
780  
781        // d
782 
783        rotate_three();
784        push_cube();
785        hold_cube();
786
787        rotate_two();
788        release_cube();
789        push_cube();
790        rotate_one();
791
792        push_cube();
793        rotate_two();
794        push_cube(3);
795}
796void
797  superflip() // all edges are opposite (checkered pattern)
798{
799        Serial.println();
800
801        Serial.println("Superflip: ");
802        up();
803        up();
804        down();
805
806        down();
807 
808        left();
809        left();
810        right();
811
812        right();
813 
814 
815        front();
816        front();
817        back();
818
819        back();
820}
821 
822                                                                                                                                                                                //
823  test it
824void scramble() // random 25 moves
825{
826        Serial.println();
827
828        Serial.println("Scramble: ");
829        int move;
830        for(int
831  j = 0; j < 25; j++)
832        {
833                move = random(1, 12);
834                //Serial.println(move);
835
836                switch(move)
837                {
838                        case
839  1:
840                                right();
841                                break;
842
843                        case 2:
844                                right_inverted();
845
846                                break;
847                        case 3:
848                                left();
849
850                                break;
851                        case 4:
852                                left_inverted();
853
854                                break;
855                        case 5:
856                                up();
857
858                                break;
859                        case 6:
860                                up_inverted();
861
862                                break;
863                        case 7:
864                                down();
865
866                                break;
867                        case 8:
868                                down_inverted();
869
870                                break;
871                        case 9:
872                                front();
873
874                                break;
875                        case 10:
876                                front_inverted();
877
878                                break;
879                        case 11:
880                                back();
881
882                                break;
883                        case 12:
884                                back_inverted();
885
886                                break;
887                }
888        }
889}
890
891  
892// test all possible rotation combinations (for mechanical testing)
893void
894  rotate_one_to_two()
895{
896        Serial.println("rotate_one to rotate_two");
897
898        rotate_one();
899        hold_cube();
900        rotate_two();
901        release_cube();
902
903        push_cube();
904}
905 
906void rotate_two_to_one()
907{
908        Serial.println("rotate_two
909  to rotate_one");
910        rotate_two();
911        hold_cube();
912        rotate_one();
913
914        release_cube();
915        push_cube();
916}
917 
918void rotate_two_to_three()
919{
920
921        Serial.println("rotate_two to rotate_three");
922        rotate_two();
923
924        hold_cube();
925        rotate_three();
926        release_cube();
927        push_cube();
928}
929
930  
931void rotate_three_to_two()
932{
933        Serial.println("rotate_three to
934  rotate_two");
935        rotate_three();
936        hold_cube();
937        rotate_two();
938
939        release_cube();
940        push_cube();
941}
942 
943// double turns:
944void
945  rotate_three_to_one()
946{
947        Serial.println("rotate_three to rotate_two");
948
949        rotate_three();
950        hold_cube();
951        rotate_one();
952        release_cube();
953
954        push_cube();
955}
956 
957void rotate_one_to_three()
958{
959        Serial.println("rotate_one
960  to rotate_two");
961        rotate_one();
962        hold_cube();
963        rotate_three();
964
965        release_cube();
966        push_cube();
967}
968void rotation_test()
969{
970
971        Serial.println("Rotation Test:");
972 
973        rotate_one_to_two();
974
975        rotate_two_to_one();
976        rotate_two_to_three();
977        rotate_three_to_two();
978
979        rotate_three_to_one();
980        rotate_one_to_three();
981 
982        rotate_one();
983
984        push_cube();
985        rotate_two();
986        push_cube();
987        rotate_three();
988
989        push_cube();
990       
991}
992
993
994 
995
996void run_kociemba(){
997
998
999//
1000  Length (with one extra character for the null terminator)
1001int str_len = kociemba_sol.length()
1002  + 1; 
1003
1004
1005Serial.println("arduino dice: Caracteres:");
1006Serial.println((str_len-1));
1007
1008for(int
1009  i = 0; i <= (str_len-1); i++){     //recorre
1010
1011//Serial.print(i);
1012
1013
1014if
1015  ((kociemba_sol.charAt(i)) =='R'){
1016
1017
1018if ((kociemba_sol.charAt(i+1)) == '2')
1019  {
1020      right();
1021      right();}
1022
1023
1024if ((kociemba_sol.charAt(i+1))
1025  == '\\'') {
1026      right_inverted();}
1027
1028if (((kociemba_sol.charAt(i+1))!=
1029  '2') and((kociemba_sol.charAt(i+1)) != '\\'')) {
1030      right();}
1031
1032}
1033
1034else
1035  { //NO HACER NADA
1036      
1037}
1038
1039if ((kociemba_sol.charAt(i)) =='L'){
1040
1041
1042if
1043  ((kociemba_sol.charAt(i+1)) == '2') {
1044      left();
1045      left();}
1046
1047
1048if
1049  ((kociemba_sol.charAt(i+1)) == '\\'') {
1050      left_inverted();}
1051
1052if (((kociemba_sol.charAt(i+1))!=
1053  '2') and((kociemba_sol.charAt(i+1)) != '\\'')) {
1054      left();}
1055
1056}
1057
1058else
1059  { //NO HACER NADA
1060      
1061}
1062
1063if ((kociemba_sol.charAt(i)) =='U'){
1064
1065
1066if
1067  ((kociemba_sol.charAt(i+1)) == '2') {
1068      up();
1069      up();}
1070
1071
1072if
1073  ((kociemba_sol.charAt(i+1)) == '\\'') {
1074      up_inverted();}
1075
1076if (((kociemba_sol.charAt(i+1))!=
1077  '2') and((kociemba_sol.charAt(i+1)) != '\\'')) {
1078      up();}
1079
1080}
1081
1082else
1083  { //NO HACER NADA
1084      
1085}
1086
1087
1088
1089if ((kociemba_sol.charAt(i)) =='D'){
1090
1091
1092if
1093  ((kociemba_sol.charAt(i+1)) == '2') {
1094      down();
1095      down();}
1096
1097
1098if
1099  ((kociemba_sol.charAt(i+1)) == '\\'') {
1100      down_inverted();}
1101
1102if (((kociemba_sol.charAt(i+1))!=
1103  '2') and((kociemba_sol.charAt(i+1)) != '\\'')) {
1104      down();}
1105
1106}
1107
1108else
1109  { //NO HACER NADA
1110      
1111}
1112
1113
1114
1115
1116
1117if ((kociemba_sol.charAt(i))
1118  =='F'){
1119
1120
1121if ((kociemba_sol.charAt(i+1)) == '2') {
1122      front();
1123
1124      front();}
1125
1126
1127if ((kociemba_sol.charAt(i+1)) == '\\'') {
1128      front_inverted();}
1129
1130if
1131  (((kociemba_sol.charAt(i+1))!= '2') and((kociemba_sol.charAt(i+1)) != '\\'')) {
1132
1133      front();}
1134
1135}
1136
1137else { //NO HACER NADA
1138      
1139}
1140
1141
1142
1143
1144if
1145  ((kociemba_sol.charAt(i)) =='B'){
1146
1147
1148if ((kociemba_sol.charAt(i+1)) == '2')
1149  {
1150      back();
1151      back();}
1152
1153
1154if ((kociemba_sol.charAt(i+1)) ==
1155  '\\'') {
1156      back_inverted();}
1157
1158if (((kociemba_sol.charAt(i+1))!= '2')
1159  and((kociemba_sol.charAt(i+1)) != '\\'')) {
1160      back();}
1161
1162}
1163
1164else
1165  { //NO HACER NADA
1166      
1167}
1168
1169}
1170   
1171                
1172        //warm_up();
1173
1174        //scramble();
1175}
1176 
1177void show_off_cube()
1178{
1179        rotate_one();
1180
1181        rotate_three();
1182        push_cube(2);
1183        rotate_one();
1184}
1185////////////////////////////////////////////////////////////////////////////////////////////////////
1186//////////////////////////////////////
1187  PROGRAM START ///////////////////////////////////////////////
1188////////////////////////////////////////////////////////////////////////////////////////////////////
1189
1190  
1191void setup()
1192{
1193        rotate_servo.attach(9);  // attaches the servo
1194  on pin 9 to the servo object
1195        push_servo.attach(6);  // attaches the servo
1196  on pin 6 to the servo object
1197        push_servo.write(push_pos);
1198        rotate_servo.write(rotate_pos);
1199
1200        delay(1000);
1201        Serial.begin(9600);
1202        while (! Serial);
1203  // Wait untilSerial is ready
1204       
1205}
1206 
1207/////////////// Se recibe por
1208  puerto serial la solucion  //////////////////
1209
1210void loop()
1211{      
1212//recibir
1213  la solucion de kociemba por puerto serial
1214
1215
1216accept_string();
1217
1218
1219
1220  //calentando 5 segundos
1221
1222Serial.println("Calentando: volvera a la posicion
1223  inicial");
1224
1225
1226push_cube();
1227push_cube();
1228push_cube();
1229push_cube();
1230
1231delay(2000);
1232
1233
1234Serial.println("Arduino
1235  dice:Inicia a correr la solucion:");
1236
1237
1238 run_kociemba(); //corre el string
1239  recibido
1240        
1241Serial.println("Arduino dice: Finaliza tiempo solucion");
1242
1243//mostrar
1244  el cubo resuelto
1245
1246show_off_cube();
1247
1248
1249Serial.println("Arduino dice:Finalizado
1250  enviar nueva solucion de Kociemba");
1251  
1252        
1253        while(true){}
1254
1255
1256  
1257 
1258}