import java.io.IOException; import lejos.nxt.*; import lejos.nxt.comm.*; import java.io.*; import javax.bluetooth.*; // Author: ¶¯Á¦ÀÏÄк¢ // Site: http://www.diy-robots.com // Date: 2010/01/15 // Location: Beijing 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; 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.RotateBottom(1); break; case 4: Robot.RotateBottomSide(-1); break; default: return; } } } public static class Robot { 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); } public static void RotateBottom(int nQuarter)throws Exception { bottom.rotate(nQuarter * BaseOneQuarter); } 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); } } }