NXT端的程序下載源代碼

1、NXT端的程序下載(源代碼) import java.io.IOException;import lejos.nxt.*;import m.*;import java.io.*;import javax.bluetooth.*;/ Author: 動力老男孩/ Site: http:/www.diy-/ 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

2、/01/24 - Add codes to scan 6 sides of cube/ 2010/02/02 - Add bluetooth connect to PC/ 2010/04/01 - Add Error control and playing sound/ 2010/05/01 - Rewrite some codes to make it more beautifulpublic class RubikSolverV2 /Define Sensorsstatic UltrasonicSensor distance=new UltrasonicSensor(SensorPort.

3、S1);static LightSensor light = new LightSensor(SensorPort.S2);static ColorSensor color = new ColorSensor(SensorPort.S3);/Define Motorsstatic 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 = tr

4、ue;/the motor angle for paw to hold the cubestatic int PawHoldPosition = 56;/the motor angle for paw to rotate the cubestatic int PawTurnOverPosition = 110;/the motor angle for base to rotate 90 degreestatic int BaseOneQuarter = 315;/the fix angle for basestatic int BaseRotateFix = 40;/the fix angle

5、 of base position fixstatic 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 motorstatic int ColorMotorOffset1 = 33;static int ColorMotorOffset2 = 9;static int ColorMotorOf

6、fset3 = 18;static int ColorReadPostion1 = 162;static int ColorReadPostion2 = 154;/A flag to check if the cube is on the basestatic boolean hasError = false; public static void main (String arg) throws Exception light.setFloodlight(false); bottom.setSpeed(1000); BlueTooth.Connect(); boolean hasCube =

7、 true; /flag used to get the distance boolean isChaotic = true; /flag used to record a chaotic cube while(!Button.ESCAPE.isPressed() /Wait for the distance being in the correct range: 1216 int CheckStatusTimes=0;LCD.clear();boolean previousStatus = true;boolean currentStatus = true;while(CheckStatus

8、Times+ < 10)int n = distance.getDistance();LCD.drawString("distance=" + n + " ", 0, 0);currentStatus = (n>=12 && n<=16);if(currentStatus != previousStatus)CheckStatusTimes = 0;previousStatus = currentStatus;Thread.sleep(100);ha

9、sCube = currentStatus;if(!hasCube)/if the cube is take away, we consume it is been upsetisChaotic = true; if(hasCube && isChaotic) /The cube is read, init the error status hasError = false; /Play some sound to notice the "Start"Thread.sleep(1000);Sound.twoBeeps();Th

10、read.sleep(1000);Sound.p laySample(new File("Start.wav");/Fix anglesRobot.FixBasePosition(); Robot.FixColorSensorPosition(); /Read Colors Robot.ReadAllSide(); if(!hasError) /Send 255 to start calculate BlueTooth.WriteBytes(new byte(byte)255); /The first return byte is the length of

11、 steps byte readStepCount = BlueTooth.ReadBytes(); int stepCount = readStepCount0; LCD.drawString("steps=" + stepCount, 1, 1); /Start to action for(int i=0; i<stepCount; i+) if(!Robot.CheckCubeReady() break; /Fix angle every 10 steps if(i % 10 = 0) Robot.FixBasePosition(); /

12、Send request for the i step BlueTooth.WriteBytes(new byte(byte)i); /Get result int step = BlueTooth.ReadBytes()0; if(step=10) /Rotate paw Robot.RotatePaw(); else if(step>=20 && step<30) /Rotate Bottom int count = step - 20; if(count = 3) count = -1; Robot.RotateBottom(c

13、ount); else if(step>=30 && step<40) /Rotate Bottom Side int count = step - 30; if(count = 3) count = -1; Robot.RotateBottomSide(count); /Send 254 to reset the pc data BlueTooth.WriteBytes(new byte(byte)254); if(!hasError) /The cube has been solved isChaotic = false; Sou

14、nd.playSample(new File("End.wav");Thread.sleep(1000);Robot.RotateBottom(8); Thread.sleep(500); BlueTooth.Disconnect(); public static class BlueTooth static BTConnection btc ; static DataInputStream dis; static DataOutputStream dos; public static void Connect() throws Exception LCD.

15、clear(); LCD.drawString("Waiting BTC.",0,0); btc = Bluetooth.waitForConnection(); LCD.drawString("Connected",0,2); LCD.refresh(); dis = btc.openDataInputStream(); dos = btc.openDataOutputStream(); public static void Disconnect() throws Exception if(btc!=null)Write

16、Bytes(new byte(byte)255,(byte)255,(byte)255);Thread.sleep(100);dos.close();dis.close();btc.close(); public static void WriteBytes(byte data) throws Exception for(int i=0;i<data.length;i+) dos.writeByte(datai); dos.flush(); public static byte ReadBytes() throws Exception byte buffer = new byte

17、255; int length = btc.read(buffer, buffer.length); if(length=-2) /lost data, re-sync btc.read(null, 255); return new byte0; else byte data = new bytelength; for(int i=0;i<length;i+) datai = bufferi; return data; public static class Robot /Rotate bottom side of cubepublic static void RotateBot

18、tomSide(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);

19、bottom.rotate(-nFixAngle);paw.rotateTo(0);/Rotate the whole cube from bottom, without hold the armpublic static void RotateBottom(int nQuarter)throws Exception bottom.rotate(nQuarter * BaseOneQuarter);/Rotate the whole cube from pawpublic static void RotatePaw()throws Exception int nPawHoldPosition

20、= 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.setS

21、peed(400);paw.rotateTo(nPawHoldPosition);paw.setSpeed(1000);paw.rotateTo(0);/Fix the position of cube basepublic static void FixBasePosition() throws Exceptionint step = 3;int tolerance = 4;light.setFloodlight(false);bottom.rotate(-50);int angle = 0, minLight = 10000;int realtimeLight = ReadLightDif

22、ference();while(realtimeLight < minLight + tolerance)bottom.rotate(step);realtimeLight = ReadLightDifference();if(realtimeLight < minLight)minLight = realtimeLight;angle = 0;elseangle += step;bottom.rotate(- angle/2 - FixBasePositionOffset);/Read the light difference between light on a

23、nd light offprivate static int ReadLightDifference() throws Exceptionint 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 positionpublic static void FixColorSensorPosition() throws Except

24、ionint 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 + toleranc

25、e && 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() t

26、hrows Exception/Rotate the 6 sides in sequenceint nSideIndex=0;ReadOneSide(nSideIndex+);if(CheckCubeReady() RotatePaw();ReadOneSide(nSideIndex+);if(CheckCubeReady() RotatePaw();ReadOneSide(nSideIndex+);if(CheckCubeReady() RotatePaw();ReadOneSide(nSideIndex+);RotateBottom(-1);if(CheckCubeReady() Rota

27、tePaw();ReadOneSide(nSideIndex+);if(CheckCubeReady() RotatePaw();if(CheckCubeReady() RotatePaw();ReadOneSide(nSideIndex+);/Read one side by the indexpublic static void ReadOneSide(int nSideIndex) throws Exception/Add a delay time for the motor to be stableint delay=120; int idx=4,6,7,8,5,2,1,0,3,4,0

28、,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 Colormonitor.rotateTo(ColorMotorBaseAngle - ColorMotorOffset1);Thread.sleep(delay);SendColorToPC(idx2nSideIndex, idxnSideIndexi

29、+);/Read Bordersfor(int jj=0;jj<4;jj+)monitor.rotateTo(ColorMotorBaseAngle - ColorMotorOffset2);Thread.sleep(delay);SendColorToPC(idx2nSideIndex, idxnSideIndexi+);monitor.rotateTo(ColorMotorBaseAngle - ColorMotorOffset3);bottom.rotate(-ColorReadPostion1);Thread.sleep(delay);SendColorToPC(idx2nSideIndex, idxnSideIndexi+);bottom.rotate(-ColorReadPostion2);monitor.rotate