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/18 - Fix position for cube base and color sensor motor 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; 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.RotatePaw(); break; case 2: Robot.FixBasePosition(); break; case 4: 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); } } }