import java.io.IOException; import lejos.nxt.*; import lejos.nxt.comm.*; import java.io.*; import javax.bluetooth.*; // Author: ¶¯Á¦ÀÏÄк¢ // Site: http://www.diy-robots.com // Location: Beijing // Update history: // 2010/01/15 - Motor actions to rotate cube // 2010/01/16 - Fix position for cube base and color sensor motor // 2010/01/24 - Add codes to scan 6 sides of cube public class RubikSolverV2 { //Define Sensors static UltrasonicSensor distance=new UltrasonicSensor(SensorPort.S1); static LightSensor light = new LightSensor(SensorPort.S2); static ColorSensor color = new ColorSensor(SensorPort.S3); //Define Motors static Motor paw=Motor.A; static Motor bottom=Motor.C; static Motor monitor=Motor.B; //if the paw has the 3:1 reducer, set it as true static boolean HasReducer = true; //the motor angle for paw to hold the cube static int PawHoldPosition = 56; //the motor angle for paw to rotate the cube static int PawTurnOverPosition = 110; //the motor angle for base to rotate 90 degree static int BaseOneQuarter = 315; //the fix angle for base static int BaseRotateFix = 40; //the fix angle of base position fix static int FixBasePositionOffset = 16; //the init position of color sensor motor(this will be set automatically) static int ColorMotorBaseAngle = 0; //add offset positions for color sensor motor static int ColorMotorOffset1 = 33; static int ColorMotorOffset2 = 9; static int ColorMotorOffset3 = 18; static int ColorReadPostion1 = 162; static int ColorReadPostion2 = 154; public static void main (String[] arg) throws Exception { light.setFloodlight(false); bottom.setSpeed(1000); while(true) { Button.waitForPress(); int nButtonNum = Button.readButtons(); // 1:Enter; 2:Left; 3:Right; 4:Escape switch(nButtonNum) { case 1: Robot.ReadAllSide(); break; case 2: Robot.FixBasePosition(); break; case 3: Robot.FixColorSensorPosition(); break; default: return; } } } public static class Robot { //Rotate bottom side of cube public static void RotateBottomSide(int nQuarter) throws Exception { paw.setSpeed(400); int nFixAngle = BaseRotateFix * ( nQuarter > 0 ? 1 : -1); int nPawHoldPosition = PawHoldPosition; if(HasReducer) nPawHoldPosition = -3 * nPawHoldPosition; paw.rotateTo(nPawHoldPosition); bottom.rotate(nQuarter * BaseOneQuarter + nFixAngle); bottom.rotate(-nFixAngle); paw.rotateTo(0); } //Rotate the whole cube from bottom, without hold the arm public static void RotateBottom(int nQuarter)throws Exception { bottom.rotate(nQuarter * BaseOneQuarter); } //Rotate the whole cube from paw public static void RotatePaw()throws Exception { int nPawHoldPosition = PawHoldPosition - 8; if(HasReducer) nPawHoldPosition = -3 * nPawHoldPosition; int nPawTurnOverPosition = PawTurnOverPosition; if(HasReducer) nPawTurnOverPosition = -3 * nPawTurnOverPosition; paw.setSpeed(1000); paw.rotateTo(nPawHoldPosition); paw.setSpeed(300); paw.rotateTo(nPawTurnOverPosition); paw.setSpeed(400); paw.rotateTo(nPawHoldPosition); paw.setSpeed(1000); paw.rotateTo(0); } //Fix the position of cube base public static void FixBasePosition() throws Exception { int step = 3; int tolerance = 4; light.setFloodlight(false); bottom.rotate(-50); int angle = 0, minLight = 10000; int realtimeLight = ReadLightDifference(); while(realtimeLight < minLight + tolerance) { bottom.rotate(step); realtimeLight = ReadLightDifference(); if(realtimeLight < minLight) { minLight = realtimeLight; angle = 0; } else { angle += step; } } bottom.rotate(- angle/2 - FixBasePositionOffset); } //Read the light difference between light on and light off private static int ReadLightDifference() throws Exception { int l1 = 0, l2 = 0; l1 = light.readValue(); light.setFloodlight(true); Thread.sleep(20); l2 = light.readValue(); light.setFloodlight(false); return l1-l2; } //Fix color sensor position public static void FixColorSensorPosition() throws Exception { int tolerance = 5; ColorMotorBaseAngle = -25; monitor.rotateTo(ColorMotorBaseAngle); Thread.sleep(100); monitor.setSpeed(50); int r = color.getRawRed(); int g = color.getRawGreen(); int b = color.getRawBlue(); int baseColor = r + g + b; int TargetExists = 0; while(TargetExists < baseColor + tolerance && ColorMotorBaseAngle > -50) { monitor.rotateTo(ColorMotorBaseAngle--); r = color.getRawRed(); g = color.getRawGreen(); b = color.getRawBlue(); TargetExists = r + g + b; } monitor.rotateTo(ColorMotorBaseAngle + 32); } //Read each side colors of the cube public static void ReadAllSide() throws Exception { //Rotate the 6 sides in sequence int nSideIndex=0; ReadOneSide(nSideIndex++); RotatePaw(); ReadOneSide(nSideIndex++); RotatePaw(); ReadOneSide(nSideIndex++); RotatePaw(); ReadOneSide(nSideIndex++); RotateBottom(-1); RotatePaw(); ReadOneSide(nSideIndex++); RotatePaw(); RotatePaw(); ReadOneSide(nSideIndex++); } //Read one side by the index public static void ReadOneSide(int nSideIndex) throws Exception { //Add a delay time for the motor to be stable int delay=120; int[][] idx={ {4,6,7,8,5,2,1,0,3}, {4,0,3,6,7,8,5,2,1}, {4,2,1,0,3,6,7,8,5}, {4,8,5,2,1,0,3,6,7}, {4,2,1,0,3,6,7,8,5}, {4,2,1,0,3,6,7,8,5}}; int[] idx2={5,1,4,3,2,0}; int i=0; monitor.setSpeed(200); bottom.setSpeed(1000); //Read Center Color monitor.rotateTo(ColorMotorBaseAngle - ColorMotorOffset1); Thread.sleep(delay); SendColorToPC(idx2[nSideIndex], idx[nSideIndex][i++]); //Read Borders for(int jj=0;jj<4;jj++) { monitor.rotateTo(ColorMotorBaseAngle - ColorMotorOffset2); Thread.sleep(delay); SendColorToPC(idx2[nSideIndex], idx[nSideIndex][i++]); monitor.rotateTo(ColorMotorBaseAngle - ColorMotorOffset3); bottom.rotate(-ColorReadPostion1); Thread.sleep(delay); SendColorToPC(idx2[nSideIndex], idx[nSideIndex][i++]); bottom.rotate(-ColorReadPostion2); } monitor.rotateTo(ColorMotorBaseAngle + 32); } //Send colors to PC public static void SendColorToPC(int center, int n) throws Exception { //get the x,y of n int y = n % 3; int x = (n - y) / 3; //send to PC by bluetooth, will enable them later /* dos.writeByte(center); dos.writeByte(x); dos.writeByte(y); dos.writeByte(color.getRed()); dos.writeByte(color.getGreen()); dos.writeByte(color.getBlue()); dos.writeByte(color.getRawRed()/3); dos.writeByte(color.getRawGreen()/3); dos.writeByte(color.getRawBlue()/3); dos.flush(); */ } } }