/* Tilted Twister, a Lego Mindstorms robot that solves Rubik’s cube Hans Andersson 2008 www.tiltedtwister.com ^^^^^^^^ REVISIONE (RisolviCuboV1.1) - Padovan Valter - ITALY - Feb.2009 ^^^^^^^^ http://digilander.libero.it/Valter_NXT/ The program is optimized for using 6 x 1.2V rechargeable batteries If using 1.5V batteries, the functions Tilt() and CheckBatteries() must be adjusted If you have a cube with a symbol (rubik's logo) on one of the center faces, the function FixColors must be adjusted Inputs: 1 Touch sensor 2 Light sensor 3 Ultrasonic sensor Outputs: A Center motor B Tilter arm motor C Light sensor motor */ #include "NXCDefs.h" #define MINBATTERYLEVEL 6000 // ^^^^^^^ Riga aggiunta #define MAXBATTERYLEVEL 8400 // ^^^^^^^ Riga aggiunta #define TWISTSPEED 100 #define OVERTWIST 15 #define LEFTFACE 0 #define FRONTFACE 1 #define RIGHTFACE 2 #define BACKFACE 3 #define UPPERFACE 4 #define DOWNFACE 5 #define UPPERLEFT 0 #define UPPERMID 1 #define UPPERRIGHT 2 #define MIDLEFT 3 #define CENTER 4 #define MIDRIGHT 5 #define DOWNLEFT 6 #define DOWNMID 7 #define DOWNRIGHT 8 #define LEFTFACE_UPPERLEFT 0 #define LEFTFACE_UPPERMID 1 #define LEFTFACE_UPPERRIGHT 2 #define LEFTFACE_MIDLEFT 3 #define LEFTFACE_CENTER 4 #define LEFTFACE_MIDRIGHT 5 #define LEFTFACE_DOWNLEFT 6 #define LEFTFACE_DOWNMID 7 #define LEFTFACE_DOWNRIGHT 8 #define FRONTFACE_UPPERLEFT 9 #define FRONTFACE_UPPERMID 10 #define FRONTFACE_UPPERRIGHT 11 #define FRONTFACE_MIDLEFT 12 #define FRONTFACE_CENTER 13 #define FRONTFACE_MIDRIGHT 14 #define FRONTFACE_DOWNLEFT 15 #define FRONTFACE_DOWNMID 16 #define FRONTFACE_DOWNRIGHT 17 #define RIGHTFACE_UPPERLEFT 18 #define RIGHTFACE_UPPERMID 19 #define RIGHTFACE_UPPERRIGHT 20 #define RIGHTFACE_MIDLEFT 21 #define RIGHTFACE_CENTER 22 #define RIGHTFACE_MIDRIGHT 23 #define RIGHTFACE_DOWNLEFT 24 #define RIGHTFACE_DOWNMID 25 #define RIGHTFACE_DOWNRIGHT 26 #define BACKFACE_UPPERLEFT 27 #define BACKFACE_UPPERMID 28 #define BACKFACE_UPPERRIGHT 29 #define BACKFACE_MIDLEFT 30 #define BACKFACE_CENTER 31 #define BACKFACE_MIDRIGHT 32 #define BACKFACE_DOWNLEFT 33 #define BACKFACE_DOWNMID 34 #define BACKFACE_DOWNRIGHT 35 #define UPPERFACE_UPPERLEFT 36 #define UPPERFACE_UPPERMID 37 #define UPPERFACE_UPPERRIGHT 38 #define UPPERFACE_MIDLEFT 39 #define UPPERFACE_CENTER 40 #define UPPERFACE_MIDRIGHT 41 #define UPPERFACE_DOWNLEFT 42 #define UPPERFACE_DOWNMID 43 #define UPPERFACE_DOWNRIGHT 44 #define DOWNFACE_UPPERLEFT 45 #define DOWNFACE_UPPERMID 46 #define DOWNFACE_UPPERRIGHT 47 #define DOWNFACE_MIDLEFT 48 #define DOWNFACE_CENTER 49 #define DOWNFACE_MIDRIGHT 50 #define DOWNFACE_DOWNLEFT 51 #define DOWNFACE_DOWNMID 52 #define DOWNFACE_DOWNRIGHT 53 #define MOVES_SIZE 500 int color[6*9]; char cube[6*9]; char tmpCube[6*9]; char moves[MOVES_SIZE]; int movesCount=0; char solution[MOVES_SIZE]; int solutionCount; int solutionTwists; int twists; char staticfaces[]={'L','F','R','B','U','D'}; char faces[]={'L','F','R','B','U','D'}; sub CubeSet(int face, int pos, char value) { cube[face * 9 + pos] = value; } char CubeGet(int face, int pos) { return cube[face * 9 + pos]; } char TmpCubeGet(int face, int pos) { return tmpCube[face * 9 + pos]; } sub Turn(int num) { if (num==3) RotateMotor(OUT_A,100,-315); else RotateMotor(OUT_A,100,315*num); for(int i=0;i 9) { PlayFile("Woops.rso"); while(true) { US=SensorUS(IN_3); if(US >= 7 && US <= 9) { Wait(3000); US=SensorUS(IN_3); if(US >= 7 && US <= 9) break; } } PlayFile("Thank You.rso"); Wait(2000); } } sub Tilt() { // This function is optimized for using 6 x 1.2V rechargeable batteries // If using 1.5V batteries, it has to be adjusted CheckCube(); RotateMotorEx(OUT_B,50,40,false,false,false); RotateMotorEx(OUT_B,80,8,false,false,false); RotateMotorEx(OUT_B,100,7,false,false,false); RotateMotorEx(OUT_B,40,10,false,false,true); RotateMotor(OUT_B,30,-65); char uf=faces[UPPERFACE]; faces[UPPERFACE]=faces[LEFTFACE]; faces[LEFTFACE]=faces[DOWNFACE]; faces[DOWNFACE]=faces[RIGHTFACE]; faces[RIGHTFACE]=uf; } sub Twist(int num) { CheckCube(); RotateMotorPID(OUT_B,50,40,100,0,0); switch(num) { case 1: RotateMotorEx(OUT_A,TWISTSPEED,315 + OVERTWIST,0,false,false); RotateMotor(OUT_A,TWISTSPEED, -1 * OVERTWIST); break; case 2: RotateMotorEx(OUT_A,TWISTSPEED,315 * 2 + OVERTWIST,0,false,false); RotateMotor(OUT_A,TWISTSPEED, -1 * OVERTWIST); break; case 3: RotateMotorEx(OUT_A,TWISTSPEED,-315 - OVERTWIST,0,false,false); RotateMotor(OUT_A,TWISTSPEED,OVERTWIST); break; } RotateMotorEx(OUT_B,20,-40,0,false,false); } sub ColorSet(int face, int pos,int col) { color[face*9+pos]=col; } int ColorGet(int face, int pos) { return color[face*9+pos]; } int ReadColor(int face, int pos) { int light; light=Sensor(IN_2); ColorSet(face,pos,light); return light; } sub TurnNoStop() { RotateMotorEx(OUT_A,100,315,false,false,false); } // NB aggiunta tutta la SUB sub InitPositionLight() // ^^^^^^^ Riga aggiunta { // ^^^^^^^ Riga aggiunta OnFwd(OUT_C,10); // ^^^^^^^ Riga aggiunta until(SENSOR_1 == 0); // ^^^^^^^ Riga aggiunta Off(OUT_C); // ^^^^^^^ Riga aggiunta OnRev(OUT_C,10); // ^^^^^^^ Riga aggiunta until(SENSOR_1 == 1); // ^^^^^^^ Riga aggiunta Off(OUT_C); // ^^^^^^^ Riga aggiunta RotateMotor(OUT_C,10,26); // ^^^^^^^ Riga aggiunta } // ^^^^^^^ Riga aggiunta sub ScanFace(int face, string title) { int light; TextOut(20,LCD_LINE2,title,true); //Center RotateMotor(OUT_C,20,110); // ^^^^^^^ Riga modificata (era 125°) light = ReadColor(face,CENTER); NumOut(40,LCD_LINE5,light); //Edges RotateMotor(OUT_C,20,-25); // ^^^^^^^ Riga modificata (era -17°) RotateMotor(OUT_A,80,35); light = ReadColor(face,DOWNMID); NumOut(40,LCD_LINE6,light); TurnNoStop(); light = ReadColor(face,MIDLEFT); NumOut(10,LCD_LINE5,light); TurnNoStop(); light = ReadColor(face,UPPERMID); NumOut(40,LCD_LINE4,light); Turn(1); light = ReadColor(face,MIDRIGHT); NumOut(70,LCD_LINE5,light); //Corners RotateMotor(OUT_C,20,-8); RotateMotor(OUT_A,80,160); light = ReadColor(face,DOWNRIGHT); NumOut(70,LCD_LINE6,light); TurnNoStop(); light = ReadColor(face,DOWNLEFT); NumOut(10,LCD_LINE6,light); TurnNoStop(); light = ReadColor(face,UPPERLEFT); NumOut(10,LCD_LINE4,light); Turn(1); light = ReadColor(face,UPPERRIGHT); NumOut(70,LCD_LINE4,light); RotateMotor(OUT_C,20,-60); // ^^^^^^^ Riga modificata (era -100°) RotateMotor(OUT_A,80,120); } sub FixColors() { int col,face,pos,i,maxvalue,maxface,maxpos; //Center for(col = 0;col < 6; col++) { maxvalue=0; for(face=0;face<6;face++) if(ColorGet(face,CENTER) > maxvalue) { maxvalue=ColorGet(face,CENTER); maxface=face; } ColorSet(maxface,CENTER,col); // If you have a cube with a symbol (rubik's logo) on one of the center faces, special care has to be taken. // Uncomment the following section and make adjustments depending on how your cube is colored // On my cube, the rubik's logo sticker is on the opposite side of the color with the second highest light sensor value /* if(col==1) { switch(maxface) { case UPPERFACE: ColorSet(DOWNFACE,CENTER,2);break; case LEFTFACE: ColorSet(RIGHTFACE,CENTER,2);break; case FRONTFACE: ColorSet(BACKFACE,CENTER,2);break; case RIGHTFACE: ColorSet(LEFTFACE,CENTER,2);break; case BACKFACE: ColorSet(FRONTFACE,CENTER,2);break; case DOWNFACE: ColorSet(UPPERFACE,CENTER,2);break; } col++; } */ } //Edges for(col = 0;col < 6; col++) for(i=0;i<4;i++) { maxvalue=0; for(face=0;face<6;face++) for(pos=UPPERMID;pos<=DOWNMID;pos+=2) if(ColorGet(face,pos) > maxvalue) { maxvalue=ColorGet(face,pos); maxface=face; maxpos=pos; } ColorSet(maxface,maxpos,col); } //Corners for(col = 0;col < 6; col++) for(i=0;i<4;i++) { maxvalue=0; for(face=0;face<6;face++) for(pos=UPPERLEFT;pos<=DOWNRIGHT;pos+=2) if(pos!=CENTER && ColorGet(face,pos) > maxvalue) { maxvalue=ColorGet(face,pos); maxface=face; maxpos=pos; } ColorSet(maxface,maxpos,col); } } sub ScanCube() { SetSensorTouch(IN_1); SetSensorLight(IN_2); SetSensorMode(IN_2,SENSOR_MODE_RAW); //N.B. Tolto tutto il codice messo nella SUB InitPositionLight InitPositionLight(); // ^^^^^^^ Riga aggiunta ScanFace(LEFTFACE,"LEFT FACE"); Tilt(); Turn(1); InitPositionLight(); // ^^^^^^^ Riga aggiunta ScanFace(BACKFACE,"BACK FACE"); Tilt(); Turn(1); InitPositionLight(); // ^^^^^^^ Riga aggiunta ScanFace(RIGHTFACE,"RIGHT FACE"); Turn(3); Tilt(); Turn(1); InitPositionLight(); // ^^^^^^^ Riga aggiunta ScanFace(DOWNFACE,"DOWN FACE"); Turn(1); Tilt(); InitPositionLight(); // ^^^^^^^ Riga aggiunta ScanFace(FRONTFACE,"FRONT FACE"); Turn(1); Tilt(); InitPositionLight(); // ^^^^^^^ Riga aggiunta ScanFace(UPPERFACE,"UPPER FACE"); RotateMotor(OUT_C,20,-20); // ^^^^^^^ Riga aggiunta FixColors(); SetSensorType(IN_2,SENSOR_TYPE_NONE); } sub CopyCube() { ArraySubset(tmpCube,cube,0,54); } sub CopyFace(int fromFace,int toFace) { int fromFaceOffset=fromFace*9; int toFaceOffset=toFace*9; for(int i=0;i<9;i++) cube[toFaceOffset+i]=tmpCube[fromFaceOffset+i]; } sub CopyFaceClockwise(int fromFace, int toFace, int turns) { int fromFaceOffset = fromFace*9; int toFaceOffset = toFace*9; if(turns==1) { cube[toFaceOffset + UPPERLEFT] = tmpCube[fromFaceOffset + DOWNLEFT]; cube[toFaceOffset + UPPERMID] = tmpCube[fromFaceOffset + MIDLEFT]; cube[toFaceOffset + UPPERRIGHT] = tmpCube[fromFaceOffset + UPPERLEFT]; cube[toFaceOffset + MIDLEFT] = tmpCube[fromFaceOffset + DOWNMID]; cube[toFaceOffset + CENTER] = tmpCube[fromFaceOffset + CENTER]; cube[toFaceOffset + MIDRIGHT] = tmpCube[fromFaceOffset + UPPERMID]; cube[toFaceOffset + DOWNLEFT] = tmpCube[fromFaceOffset + DOWNRIGHT]; cube[toFaceOffset + DOWNMID] = tmpCube[fromFaceOffset + MIDRIGHT]; cube[toFaceOffset + DOWNRIGHT] =tmpCube[fromFaceOffset +UPPERRIGHT]; } else if(turns==2) { cube[toFaceOffset + UPPERLEFT] = tmpCube[fromFaceOffset + DOWNRIGHT]; cube[toFaceOffset + UPPERMID] = tmpCube[fromFaceOffset + DOWNMID]; cube[toFaceOffset + UPPERRIGHT] = tmpCube[fromFaceOffset + DOWNLEFT]; cube[toFaceOffset + MIDLEFT] = tmpCube[fromFaceOffset + MIDRIGHT]; cube[toFaceOffset + CENTER] = tmpCube[fromFaceOffset + CENTER]; cube[toFaceOffset + MIDRIGHT] = tmpCube[fromFaceOffset + MIDLEFT]; cube[toFaceOffset + DOWNLEFT] = tmpCube[fromFaceOffset + UPPERRIGHT]; cube[toFaceOffset + DOWNMID] = tmpCube[fromFaceOffset + UPPERMID]; cube[toFaceOffset + DOWNRIGHT] =tmpCube[fromFaceOffset +UPPERLEFT]; } else //turns==3 { cube[toFaceOffset + UPPERLEFT] = tmpCube[fromFaceOffset + UPPERRIGHT]; cube[toFaceOffset + UPPERMID] = tmpCube[fromFaceOffset + MIDRIGHT]; cube[toFaceOffset + UPPERRIGHT] = tmpCube[fromFaceOffset + DOWNRIGHT]; cube[toFaceOffset + MIDLEFT] = tmpCube[fromFaceOffset + UPPERMID]; cube[toFaceOffset + CENTER] = tmpCube[fromFaceOffset + CENTER]; cube[toFaceOffset + MIDRIGHT] = tmpCube[fromFaceOffset + DOWNMID]; cube[toFaceOffset + DOWNLEFT] = tmpCube[fromFaceOffset + UPPERLEFT]; cube[toFaceOffset + DOWNMID] = tmpCube[fromFaceOffset + MIDLEFT]; cube[toFaceOffset + DOWNRIGHT] =tmpCube[fromFaceOffset +DOWNLEFT]; } } sub TurnCube(int turns) { CopyCube(); if(turns==1) { CopyFaceClockwise(UPPERFACE,UPPERFACE,1); CopyFace(LEFTFACE,BACKFACE); CopyFace(BACKFACE,RIGHTFACE); CopyFace(RIGHTFACE,FRONTFACE); CopyFace(FRONTFACE,LEFTFACE); CopyFaceClockwise(DOWNFACE,DOWNFACE,3); } else if(turns==2) { CopyFaceClockwise(UPPERFACE,UPPERFACE,2); CopyFace(LEFTFACE,RIGHTFACE); CopyFace(BACKFACE,FRONTFACE); CopyFace(RIGHTFACE,LEFTFACE); CopyFace(FRONTFACE,BACKFACE); CopyFaceClockwise(DOWNFACE,DOWNFACE,2); } else //turns==3 { CopyFaceClockwise(UPPERFACE,UPPERFACE,3); CopyFace(LEFTFACE,FRONTFACE); CopyFace(BACKFACE,LEFTFACE); CopyFace(RIGHTFACE,BACKFACE); CopyFace(FRONTFACE,RIGHTFACE); CopyFaceClockwise(DOWNFACE,DOWNFACE,1); } } sub TiltCube(int turns) { CopyCube(); if(turns==1) { CopyFaceClockwise(UPPERFACE,RIGHTFACE,1); CopyFaceClockwise(RIGHTFACE,DOWNFACE,1); CopyFaceClockwise(DOWNFACE,LEFTFACE,1); CopyFaceClockwise(LEFTFACE,UPPERFACE,1); CopyFaceClockwise(FRONTFACE,FRONTFACE,1); CopyFaceClockwise(BACKFACE,BACKFACE,3); } else if(turns==2) { CopyFaceClockwise(UPPERFACE,DOWNFACE,2); CopyFaceClockwise(RIGHTFACE,LEFTFACE,2); CopyFaceClockwise(DOWNFACE,UPPERFACE,2); CopyFaceClockwise(LEFTFACE,RIGHTFACE,2); CopyFaceClockwise(FRONTFACE,FRONTFACE,2); CopyFaceClockwise(BACKFACE,BACKFACE,2); } else //turns==3 { CopyFaceClockwise(UPPERFACE,LEFTFACE,3); CopyFaceClockwise(RIGHTFACE,UPPERFACE,3); CopyFaceClockwise(DOWNFACE,RIGHTFACE,3); CopyFaceClockwise(LEFTFACE,DOWNFACE,3); CopyFaceClockwise(FRONTFACE,FRONTFACE,3); CopyFaceClockwise(BACKFACE,BACKFACE,1); } } sub TwistCube(int turns) { char move; for (int twists = 0; twists < turns; twists++) { CopyCube(); CopyFaceClockwise(DOWNFACE, DOWNFACE,1); for (int i = 6; i < 9; i++) { CubeSet(LEFTFACE, i, TmpCubeGet(BACKFACE, i)); CubeSet(FRONTFACE, i, TmpCubeGet(LEFTFACE, i)); CubeSet(RIGHTFACE, i, TmpCubeGet(FRONTFACE, i)); CubeSet(BACKFACE, i, TmpCubeGet(RIGHTFACE, i)); } move=cube[DOWNFACE_CENTER]; moves[movesCount++]=move; } PlayTone(200+movesCount*20,1); } sub RotateFace(int face, int turns) { switch (face) { case UPPERFACE: TiltCube(2); TwistCube(turns); TiltCube(2); break; case LEFTFACE: TiltCube(3); TwistCube(turns); TiltCube(1); break; case FRONTFACE: TurnCube(1); TiltCube(3); TwistCube(turns); TiltCube(1); TurnCube(3); break; case RIGHTFACE: TiltCube(1); TwistCube(turns); TiltCube(3); break; case BACKFACE: TurnCube(3); TiltCube(3); TwistCube(turns); TiltCube(1); TurnCube(1); break; case DOWNFACE: TwistCube(turns); break; } } sub RotateFaces(string faces) { char faceturn; for (int i = 0; i < StrLen(faces); i++) { faceturn=faces[i]; switch (faceturn) { case 'U': RotateFace(UPPERFACE, 1); break; case 'L': RotateFace(LEFTFACE, 1); break; case 'F': RotateFace(FRONTFACE, 1); break; case 'R': RotateFace(RIGHTFACE, 1); break; case 'B': RotateFace(BACKFACE, 1); break; case 'D': RotateFace(DOWNFACE, 1); break; case 'u': RotateFace(UPPERFACE, 3); break; case 'l': RotateFace(LEFTFACE, 3); break; case 'f': RotateFace(FRONTFACE, 3); break; case 'r': RotateFace(RIGHTFACE, 3); break; case 'b': RotateFace(BACKFACE, 3); break; case 'd': RotateFace(DOWNFACE, 3); break; } } } bool CornerColorOk(int position, char c1, char c2) { return cube[position] == c1 || cube[position] == c2; } bool TryBottomFace(char c1, char c2, int twists) { for (int i = 0; i < 4; i++) { if (twists == 0) { if (CornerColorOk(DOWNFACE_UPPERLEFT, c1, c2) && CornerColorOk(DOWNFACE_UPPERRIGHT, c1, c2) && CornerColorOk(DOWNFACE_DOWNRIGHT, c1, c2) && !CornerColorOk(DOWNFACE_DOWNLEFT, c1, c2)) return true; } else if (twists == 1) { if (CornerColorOk(DOWNFACE_UPPERLEFT, c1, c2) && CornerColorOk(DOWNFACE_UPPERRIGHT, c1, c2)) { for (int j = 0; j < 4; j++) { RotateFaces("B"); if (CornerColorOk(DOWNFACE_DOWNRIGHT, c1, c2) && !CornerColorOk(DOWNFACE_DOWNLEFT, c1, c2)) return true; else if (!CornerColorOk(DOWNFACE_DOWNRIGHT, c1, c2) && CornerColorOk(DOWNFACE_DOWNLEFT, c1, c2)) { TurnCube(3); return true; } else if (CornerColorOk(DOWNFACE_DOWNRIGHT, c1, c2) && CornerColorOk(DOWNFACE_DOWNLEFT, c1, c2)) { if (CornerColorOk(UPPERFACE_UPPERLEFT, c1, c2) && CornerColorOk(UPPERFACE_UPPERRIGHT, c1, c2) && CornerColorOk(UPPERFACE_DOWNLEFT, c1, c2) && CornerColorOk(UPPERFACE_DOWNRIGHT, c1, c2)) return true; } } } } else if (twists == 2) { if (CornerColorOk(DOWNFACE_UPPERLEFT, c1, c2)) { for (int j = 0; j < 4; j++) { RotateFaces("R"); if (CornerColorOk(DOWNFACE_UPPERRIGHT, c1, c2)) { for (int k = 0; k < 4; k++) { RotateFaces("B"); if (CornerColorOk(DOWNFACE_DOWNRIGHT, c1, c2) && !CornerColorOk(DOWNFACE_DOWNLEFT, c1, c2)) return true; else if (!CornerColorOk(DOWNFACE_DOWNRIGHT, c1, c2) && CornerColorOk(DOWNFACE_DOWNLEFT, c1, c2)) { TurnCube(3); return true; } } } } } } TurnCube(1); } return false; } bool PrepareBottomFace(char c1, char c2, int twists) { if (TryBottomFace(c1, c2, twists)) return true; TiltCube(1); if (TryBottomFace(c1, c2, twists)) return true; TiltCube(1); if (TryBottomFace(c1, c2, twists)) return true; TiltCube(1); if (TryBottomFace(c1, c2, twists)) return true; TurnCube(1); TiltCube(1); if (TryBottomFace(c1, c2, twists)) return true; TiltCube(2); if (TryBottomFace(c1, c2, twists)) return true; return false; } bool MoveCorners(int corner1,int corner2,int corner3,int corner4, char color1, char color2, string moves) { if (CornerColorOk(corner1, color1, color2) && CornerColorOk(corner2, color1, color2) && CornerColorOk(corner3, color1, color2) && CornerColorOk(corner4, color1, color2)) { RotateFaces(moves); return true; } return false; } sub OrientAllCorners(char c1, char c2) { if (!PrepareBottomFace(c1, c2, 0)) if (!PrepareBottomFace(c1, c2, 1)) PrepareBottomFace(c1, c2, 2); if (CornerColorOk(DOWNFACE_DOWNLEFT, c1, c2)) return; else if (CornerColorOk(LEFTFACE_DOWNLEFT, c1, c2)) { for (int i = 0; i < 4; i++) { if (MoveCorners(BACKFACE_UPPERRIGHT, RIGHTFACE_UPPERRIGHT, UPPERFACE_DOWNLEFT, UPPERFACE_DOWNRIGHT, c1, c2, "Lul")) return; else if (MoveCorners(BACKFACE_UPPERRIGHT, UPPERFACE_UPPERRIGHT, UPPERFACE_DOWNLEFT, FRONTFACE_UPPERRIGHT, c1, c2, "flF")) return; else if (MoveCorners(LEFTFACE_UPPERLEFT, UPPERFACE_UPPERRIGHT, LEFTFACE_UPPERRIGHT, RIGHTFACE_UPPERLEFT, c1, c2, "fLLF")) return; else if (MoveCorners(BACKFACE_UPPERRIGHT, UPPERFACE_UPPERRIGHT, FRONTFACE_UPPERLEFT, RIGHTFACE_UPPERLEFT, c1, c2, "LLDF")) return; else if (MoveCorners(UPPERFACE_UPPERLEFT, UPPERFACE_UPPERRIGHT, FRONTFACE_UPPERLEFT, UPPERFACE_DOWNRIGHT, c1, c2, "LfLf")) return; else if (MoveCorners(LEFTFACE_UPPERLEFT, BACKFACE_UPPERLEFT, LEFTFACE_UPPERRIGHT, UPPERFACE_DOWNRIGHT, c1, c2, "bDDLdl")) return; else if (MoveCorners(BACKFACE_UPPERRIGHT, RIGHTFACE_UPPERRIGHT, FRONTFACE_UPPERLEFT, FRONTFACE_UPPERRIGHT, c1, c2, "fLfDDb")) return; else if (MoveCorners(LEFTFACE_UPPERLEFT, BACKFACE_UPPERLEFT, FRONTFACE_UPPERLEFT, RIGHTFACE_UPPERLEFT, c1, c2, "lULfLLF")) return; RotateFaces("U"); } } else if (CornerColorOk(BACKFACE_DOWNRIGHT, c1, c2)) { for (int i = 0; i < 4; i++) { if (MoveCorners(LEFTFACE_UPPERLEFT, UPPERFACE_UPPERRIGHT, FRONTFACE_UPPERLEFT, UPPERFACE_DOWNRIGHT, c1, c2, "bUB")) return; else if (MoveCorners(LEFTFACE_UPPERLEFT, UPPERFACE_UPPERRIGHT, UPPERFACE_DOWNLEFT, RIGHTFACE_UPPERLEFT, c1, c2, "LDF")) return; else if (MoveCorners(BACKFACE_UPPERRIGHT, BACKFACE_UPPERLEFT, UPPERFACE_DOWNLEFT, FRONTFACE_UPPERRIGHT, c1, c2, "RBBr")) return; else if (MoveCorners(LEFTFACE_UPPERLEFT, RIGHTFACE_UPPERRIGHT, UPPERFACE_DOWNLEFT, FRONTFACE_UPPERRIGHT, c1, c2, "FFdB")) return; else if (MoveCorners(UPPERFACE_UPPERLEFT, RIGHTFACE_UPPERRIGHT, UPPERFACE_DOWNLEFT, UPPERFACE_DOWNRIGHT, c1, c2, "bRbr")) return; else if (MoveCorners(BACKFACE_UPPERRIGHT, BACKFACE_UPPERLEFT, LEFTFACE_UPPERRIGHT, UPPERFACE_DOWNRIGHT, c1, c2, "LUUfDF")) return; else if (MoveCorners(LEFTFACE_UPPERLEFT, RIGHTFACE_UPPERRIGHT, FRONTFACE_UPPERLEFT, RIGHTFACE_UPPERLEFT, c1, c2, "RbRDDL")) return; else if (MoveCorners(BACKFACE_UPPERRIGHT, RIGHTFACE_UPPERRIGHT, LEFTFACE_UPPERRIGHT, FRONTFACE_UPPERRIGHT, c1, c2, "FluRUUR")) return; RotateFaces("U"); } } } sub SplitCorners(char color, char oppositeColor) { int count = 0; for (int i = 0; i < 4; i++) { if (cube[DOWNFACE_UPPERLEFT] == color) count++; TurnCube(1); } if (count == 1 || count == 3) { int singleColor; if (count == 1) singleColor = color; else singleColor = oppositeColor; while (cube[DOWNFACE_UPPERLEFT] != singleColor) TurnCube(1); while (cube[UPPERFACE_DOWNRIGHT] == singleColor) RotateFaces("U"); if (cube[UPPERFACE_DOWNRIGHT] == cube[UPPERFACE_CENTER]) RotateFaces("RRDLL"); else RotateFaces("RRDRR"); } else if (count == 2) { if (cube[DOWNFACE_UPPERLEFT] != cube[DOWNFACE_DOWNRIGHT]) TiltCube(2); if (cube[DOWNFACE_UPPERLEFT] != cube[DOWNFACE_DOWNRIGHT]) { while (cube[DOWNFACE_UPPERLEFT] != cube[DOWNFACE_UPPERRIGHT]) TurnCube(1); while (cube[UPPERFACE_UPPERLEFT] != cube[DOWNFACE_UPPERLEFT] || cube[UPPERFACE_UPPERRIGHT] != cube[DOWNFACE_UPPERRIGHT]) RotateFaces("U"); if (cube[UPPERFACE_UPPERLEFT] == cube[UPPERFACE_CENTER]) RotateFaces("FF"); else RotateFaces("BB"); } else if (cube[UPPERFACE_UPPERLEFT] == cube[UPPERFACE_DOWNRIGHT]) { if (cube[UPPERFACE_UPPERLEFT] != cube[DOWNFACE_DOWNLEFT]) RotateFaces("U"); if (cube[UPPERFACE_UPPERRIGHT] == cube[UPPERFACE_CENTER]) TurnCube(1); RotateFaces("RRDDFF"); } else { while (cube[UPPERFACE_UPPERLEFT] != cube[DOWNFACE_DOWNLEFT] || cube[UPPERFACE_DOWNLEFT] != cube[DOWNFACE_DOWNLEFT]) TurnCube(1); if (cube[UPPERFACE_UPPERLEFT] != cube[UPPERFACE_CENTER]) RotateFaces("RRDRRDLL"); else RotateFaces("RRDRRDRR"); } } if (cube[UPPERFACE_UPPERLEFT] == cube[LEFTFACE_CENTER]) TiltCube(1); else if (cube[UPPERFACE_UPPERLEFT] == cube[FRONTFACE_CENTER]) { TurnCube(1); TiltCube(1); } else if (cube[UPPERFACE_UPPERLEFT] == cube[RIGHTFACE_CENTER]) TiltCube(3); else if (cube[UPPERFACE_UPPERLEFT] == cube[BACKFACE_CENTER]) { TurnCube(3); TiltCube(1); } else if (cube[UPPERFACE_UPPERLEFT] == cube[DOWNFACE_CENTER]) TiltCube(2); while (cube[UPPERFACE_UPPERLEFT] != cube[UPPERFACE_CENTER]) RotateFaces("B"); while (cube[UPPERFACE_DOWNLEFT] != cube[UPPERFACE_CENTER]) RotateFaces("F"); } //Step 3 Position all corners sub PositionAllCorners() { int count = 0; int topCount = 0; int bottomCount = 0; for (int i = 0; i < 4; i++) { if (cube[BACKFACE_DOWNLEFT] == cube[BACKFACE_DOWNRIGHT]) bottomCount++; if (cube[BACKFACE_UPPERLEFT] == cube[BACKFACE_UPPERRIGHT]) topCount++; TurnCube(1); } if (topCount > bottomCount) TiltCube(2); count = topCount + bottomCount; if (count == 0) RotateFaces("RRFFRR"); else if (count == 1) { while (cube[BACKFACE_DOWNLEFT] != cube[BACKFACE_DOWNRIGHT]) TurnCube(1); RotateFaces("RuFUUfUr"); } else if (count == 2) { while (cube[BACKFACE_DOWNLEFT] != cube[BACKFACE_DOWNRIGHT]) TurnCube(1); while (cube[BACKFACE_UPPERLEFT] != cube[BACKFACE_UPPERRIGHT]) RotateFaces("U"); RotateFaces("RRUFFUURRURR"); } else if (count == 4) RotateFaces("FFuRurUFFURUr"); else if (count == 5) { while (cube[BACKFACE_UPPERLEFT] != cube[BACKFACE_UPPERRIGHT]) TurnCube(1); RotateFaces("RuRFFrURFFRR"); } } int TopEdgesSolved() { int solved = 0; if (cube[UPPERFACE_UPPERMID] == cube[UPPERFACE_CENTER] && cube[BACKFACE_UPPERMID] == cube[BACKFACE_UPPERLEFT]) solved++; if (cube[UPPERFACE_MIDLEFT] == cube[UPPERFACE_CENTER] && cube[LEFTFACE_UPPERMID] == cube[LEFTFACE_UPPERLEFT]) solved++; if (cube[UPPERFACE_MIDRIGHT] == cube[UPPERFACE_CENTER] && cube[RIGHTFACE_UPPERMID] == cube[RIGHTFACE_UPPERLEFT]) solved++; if (cube[UPPERFACE_DOWNMID] == cube[UPPERFACE_CENTER] && cube[FRONTFACE_UPPERMID] == cube[FRONTFACE_UPPERLEFT]) solved++; return solved; } int BottomEdgesSolved() { int solved = 0; if (cube[DOWNFACE_UPPERMID] == cube[DOWNFACE_CENTER] && cube[FRONTFACE_DOWNMID] == cube[FRONTFACE_DOWNLEFT]) solved++; if (cube[DOWNFACE_MIDLEFT] == cube[DOWNFACE_CENTER] && cube[LEFTFACE_DOWNMID] == cube[LEFTFACE_DOWNLEFT]) solved++; if (cube[DOWNFACE_MIDRIGHT] == cube[DOWNFACE_CENTER] && cube[RIGHTFACE_DOWNMID] == cube[RIGHTFACE_DOWNLEFT]) solved++; if (cube[DOWNFACE_DOWNMID] == cube[DOWNFACE_CENTER] && cube[BACKFACE_DOWNMID] == cube[BACKFACE_DOWNLEFT]) solved++; return solved; } sub SetBottomFace(int downface, int downpos, int sideface, int sidepos) { if (CubeGet(downface, downpos) == cube[DOWNFACE_CENTER]) { while (cube[RIGHTFACE_DOWNLEFT] != CubeGet(sideface, sidepos)) RotateFaces("D"); } else { for (int i = 0; i < 4; i++) { if (cube[DOWNFACE_MIDRIGHT] != cube[DOWNFACE_CENTER] || cube[RIGHTFACE_DOWNMID] != cube[RIGHTFACE_DOWNLEFT]) break; RotateFaces("D"); } } } sub TopEdgeMoveOut() { for (int i = 0; i < 4; i++) { if (cube[UPPERFACE_MIDRIGHT] != cube[UPPERFACE_CENTER] || cube[RIGHTFACE_UPPERMID] != cube[RIGHTFACE_UPPERLEFT]) { SetBottomFace(0, 0, 0, 0); RotateFaces("rUdF"); return; } TurnCube(1); } } bool TopEdgeShort() { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { if (cube[LEFTFACE_MIDRIGHT] == cube[UPPERFACE_CENTER] && cube[FRONTFACE_MIDLEFT] == cube[RIGHTFACE_UPPERLEFT]) { SetBottomFace(RIGHTFACE, UPPERMID, UPPERFACE, MIDRIGHT); RotateFaces("rUdF"); return true; } if (cube[LEFTFACE_MIDLEFT] == cube[UPPERFACE_CENTER] && cube[BACKFACE_MIDRIGHT] == cube[RIGHTFACE_UPPERLEFT]) { SetBottomFace(RIGHTFACE, UPPERMID, UPPERFACE, MIDRIGHT); RotateFaces("RuDb"); return true; } if (cube[FRONTFACE_MIDLEFT] == cube[UPPERFACE_CENTER] && cube[LEFTFACE_MIDRIGHT] == cube[RIGHTFACE_UPPERLEFT]) { SetBottomFace(BACKFACE, MIDRIGHT, LEFTFACE, MIDLEFT); RotateFaces("RUUddl"); return true; } if (cube[BACKFACE_MIDRIGHT] == cube[UPPERFACE_CENTER] && cube[LEFTFACE_MIDLEFT] == cube[RIGHTFACE_UPPERLEFT]) { SetBottomFace(FRONTFACE, MIDLEFT, LEFTFACE, MIDRIGHT); RotateFaces("ruuDDL"); return true; } if (cube[RIGHTFACE_DOWNMID] == cube[UPPERFACE_CENTER] && cube[DOWNFACE_MIDRIGHT] == cube[RIGHTFACE_UPPERLEFT]) { RotateFaces("RUdf"); return true; } TurnCube(1); } RotateFaces("U"); } return false; } bool TopEdgeLong() { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { if (cube[DOWNFACE_MIDRIGHT] == cube[UPPERFACE_CENTER] && cube[RIGHTFACE_DOWNMID] == cube[RIGHTFACE_UPPERLEFT]) { SetBottomFace(BACKFACE, MIDRIGHT, LEFTFACE, MIDLEFT); RotateFaces("RuDBBUdR"); return true; } if (cube[RIGHTFACE_UPPERMID] == cube[UPPERFACE_CENTER] && cube[UPPERFACE_MIDRIGHT] == cube[RIGHTFACE_UPPERLEFT]) { SetBottomFace(LEFTFACE, MIDRIGHT, FRONTFACE, MIDLEFT); RotateFaces("ruDBBuuDDf"); return true; } TurnCube(1); } RotateFaces("U"); } return false; } //Step 4 Solve top and bottom edges sub SolveTopAndBottomEdges() { int topEdgesSolved; int bottomEdgesSolved; while (true) { topEdgesSolved = TopEdgesSolved(); bottomEdgesSolved = BottomEdgesSolved(); if (topEdgesSolved + bottomEdgesSolved >= 7) break; if (topEdgesSolved < 3 || bottomEdgesSolved == 3) if (TopEdgeShort()) continue; if (bottomEdgesSolved < 3 || topEdgesSolved == 3) { TiltCube(2); if (TopEdgeShort()) continue; } if (topEdgesSolved < 3 || bottomEdgesSolved == 3) if (TopEdgeLong()) continue; if (bottomEdgesSolved < 3 || topEdgesSolved == 3) { TiltCube(2); if (TopEdgeLong()) continue; } if (topEdgesSolved >= 3) TiltCube(2); TopEdgeMoveOut(); //If two edges are swapped on upperface } if (bottomEdgesSolved < 4) TiltCube(2); } sub PrepareMiddleEdges() { for (int i = 0; i < 4; i++) { if (cube[LEFTFACE_MIDRIGHT] == cube[UPPERFACE_CENTER] || cube[LEFTFACE_MIDLEFT] == cube[UPPERFACE_CENTER]) { while (cube[UPPERFACE_MIDRIGHT] == cube[UPPERFACE_CENTER]) RotateFaces("U"); break; } TurnCube(1); } for (int i = 0; i < 4; i++) { if (cube[UPPERFACE_MIDRIGHT] != cube[UPPERFACE_CENTER] || cube[RIGHTFACE_UPPERMID] != cube[RIGHTFACE_UPPERLEFT]) break; TurnCube(1); } } int TopEdgesInMiddleLayerOrientation() { int orientation = 0; if (cube[RIGHTFACE_MIDLEFT] != cube[LEFTFACE_CENTER] && cube[RIGHTFACE_MIDLEFT] != cube[RIGHTFACE_CENTER]) orientation = 4; if (cube[RIGHTFACE_UPPERMID] != cube[LEFTFACE_CENTER] && cube[RIGHTFACE_UPPERMID] != cube[RIGHTFACE_CENTER]) orientation += 2; if (cube[RIGHTFACE_MIDRIGHT] != cube[LEFTFACE_CENTER] && cube[RIGHTFACE_MIDRIGHT] != cube[RIGHTFACE_CENTER]) orientation += 1; return orientation; } bool MiddleEdgeTwisted(int face, int pos) { return CubeGet(face, pos) != cube[FRONTFACE_CENTER] && CubeGet(face, pos) != cube[BACKFACE_CENTER]; } int TwistedMiddleEdges() { int twisted = 0; for (int i = 0; i < 4; i++) { if (MiddleEdgeTwisted(FRONTFACE, MIDRIGHT)) twisted++; TurnCube(1); } return twisted; } //Step 5 Orient middle edges sub OrientMiddleEdges() { PrepareMiddleEdges(); if (cube[LEFTFACE_MIDRIGHT] == cube[UPPERFACE_CENTER]) { switch (TopEdgesInMiddleLayerOrientation()) { case 0: //OOO RotateFaces("UdFUdluDfUdL"); break; case 1: //OOX RotateFaces("UdfuDrUdfuDr"); break; case 2: //OXO RotateFaces("uDBUdRRUdF"); break; case 3: //OXX RotateFaces("ruDBUdrUdF"); break; case 4: //XOO RotateFaces("RRUdFuDRUdFuDr"); break; case 5: //XOX RotateFaces("rUdfuDruDBUdRuDB"); break; case 6: //XXO RotateFaces("ruDbUdRUdF"); break; case 7: //XXX RotateFaces("rUdfUdluDFFuDR"); break; } } else if (cube[LEFTFACE_MIDLEFT] == cube[UPPERFACE_CENTER]) { switch (TopEdgesInMiddleLayerOrientation()) { case 0: //OOO RotateFaces("uDbuDLUdBuDl"); break; case 1: //OOX RotateFaces("rruDbUdruDbUdR"); break; case 2: //OXO RotateFaces("UdfuDrruDb"); break; case 3: //OXX RotateFaces("RUdFuDruDb"); break; case 4: //XOO RotateFaces("uDBUdRuDBUdR"); break; case 5: //XOX RotateFaces("RuDBUdRUdfuDrUdf"); break; case 6: //XXO RotateFaces("RUdfuDRuDb"); break; case 7: //XXX RotateFaces("RuDBuDLUdbbUdr"); break; } } else if (cube[RIGHTFACE_UPPERMID] == cube[UPPERFACE_CENTER]) { switch (TwistedMiddleEdges()) { case 1: while (!MiddleEdgeTwisted(FRONTFACE, MIDRIGHT)) TurnCube(1); while(cube[UPPERFACE_MIDLEFT]==cube[UPPERFACE_CENTER]) RotateFaces("U"); RotateFaces("RUUrUUddLLuDfUUf"); break; case 3: while (MiddleEdgeTwisted(FRONTFACE, MIDRIGHT)) TurnCube(1); while (cube[UPPERFACE_MIDRIGHT] == cube[UPPERFACE_CENTER]) RotateFaces("U"); RotateFaces("ruDbuDluDf"); break; } } else if (cube[UPPERFACE_MIDRIGHT] == cube[UPPERFACE_CENTER]) { switch (TwistedMiddleEdges()) { case 2: while (true) { if (MiddleEdgeTwisted(FRONTFACE, MIDLEFT) && MiddleEdgeTwisted(FRONTFACE, MIDRIGHT)) { RotateFaces("RRFlRuRRULrf"); return; } else if (MiddleEdgeTwisted(FRONTFACE, MIDLEFT) && MiddleEdgeTwisted(BACKFACE, MIDLEFT)) { RotateFaces("FlRuRRULrfRR"); return; } TurnCube(1); } case 4: RotateFaces("RFFRRUdFUUddBRRBBUdR"); break; } } } bool Rotate3MiddleEdges() { for (int i = 0; i < 2; i++) { for (int j = 0; j < 4; j++) { if (cube[LEFTFACE_MIDRIGHT] == cube[LEFTFACE_CENTER] && cube[FRONTFACE_MIDLEFT] == cube[FRONTFACE_CENTER] && cube[FRONTFACE_MIDRIGHT] == cube[BACKFACE_CENTER] && cube[RIGHTFACE_MIDLEFT] == cube[LEFTFACE_CENTER] && cube[BACKFACE_MIDRIGHT] == cube[BACKFACE_CENTER] && cube[LEFTFACE_MIDLEFT] == cube[RIGHTFACE_CENTER]) { RotateFaces("RRuDBB"); return true; } TurnCube(1); } TiltCube(2); } return false; } bool ExchangeMiddleCenters() { bool exchangeCenters = true; for (int i = 0; i < 4; i++) { if ((cube[FRONTFACE_CENTER] != cube[BACKFACE_MIDLEFT]) || (cube[FRONTFACE_CENTER] != cube[BACKFACE_MIDRIGHT])) { exchangeCenters = false; break; } TurnCube(1); } if (exchangeCenters) RotateFaces("LLRRuDFFBB"); return exchangeCenters; } bool ExchangeMiddleCorners() { for (int i = 0; i < 2; i++) { if (cube[FRONTFACE_MIDLEFT] == cube[BACKFACE_CENTER] && cube[FRONTFACE_MIDRIGHT] == cube[BACKFACE_CENTER] && cube[BACKFACE_MIDLEFT] == cube[FRONTFACE_CENTER] && cube[BACKFACE_MIDRIGHT] == cube[FRONTFACE_CENTER] && cube[LEFTFACE_MIDLEFT] == cube[LEFTFACE_CENTER] && cube[LEFTFACE_MIDRIGHT] == cube[LEFTFACE_CENTER]) { RotateFaces("RRUUddLL"); return true; } TurnCube(1); } return false; } //Step 6 Position middle edges sub PositionMiddleEdges() { if (!Rotate3MiddleEdges()) if (!ExchangeMiddleCenters()) ExchangeMiddleCorners(); while (cube[FRONTFACE_UPPERMID] != cube[FRONTFACE_CENTER]) RotateFaces("U"); while (cube[FRONTFACE_DOWNMID] != cube[FRONTFACE_CENTER]) RotateFaces("D"); } sub Optimize() { char move; int count; int pos; int optcount; optcount=movesCount; do { twists=0; movesCount=optcount; moves[movesCount]=0; optcount=0; pos=0; while(pos < movesCount) { move = moves[pos]; count = 1; while (moves[++pos] == move) count++; count = count % 4; for(int i=0;i MAXBATTERYLEVEL || BatteryLevel() < MINBATTERYLEVEL)// ^^^^^^^ Riga modificata { TextOut(0,LCD_LINE4,"REQUIRED = 6-8V",false); TextOut(0,LCD_LINE6,"CHECK BATTERIES ",false); TextOut(0,LCD_LINE8,"USE 6*1.2V NiMH ",false); while(true); } else TextOut(0,LCD_LINE5,"GIVE ME A CUBE !"); } sub WaitForCube() { CheckBatteries(); PlayFile("Hello.rso"); Wait(1000); SetSensorLowspeed(IN_3); SensorUS(IN_3); while(true) { if(SensorUS(IN_3) < 10) { Wait(1000); if(SensorUS(IN_3) < 10) break; } } TextOut(20,LCD_LINE5,"THANK YOU !",true); PlayFile("Thank You.rso"); Wait(2000); } task main() { WaitForCube(); ScanCube(); SolveCube(); DoMoves(); }