NXT端的程序下載源代碼#優(yōu)選資料

1、NXT端的程序下載(源代碼) import java.io.IOException;import lejos.nxt.*;import m.*;import java.io.*;import javax.bluetooth.*;/ Author: 動力老男孩/ Site: / Location: Beijing/ Update history:/ 2010/01/15 - Motor actions to rotate cube/ 2010/01/16 - Fix position for cube base and c

2、olor sensor motor/ 2010/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 Ultra

3、sonicSensor(SensorPort.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

4、boolean HasReducer = true;/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 BaseRotate

5、Fix = 40;/the fix angle 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;

6、static int ColorMotorOffset3 = 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.Conne

7、ct(); boolean hasCube = 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 =

8、 true;while(CheckStatusTimes+ < 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);hasCube = curre

9、ntStatus;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();Thread.sleep(1000);Sound.p layS

10、ample(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 steps byte readStepCount = BlueTooth

11、.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(); /Send request for the i step BlueTooth.WriteBytes(

12、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(count); else if(step>=30 && step<40) /Rotate Bottom

13、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; Sound.playSample(new File("End.wav");Thread.sleep(1000);Robot.RotateBottom

14、(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.clear(); LCD.drawString("Waiting BTC.",0,0); btc = Bluetooth.waitForConnection(

15、); LCD.drawString("Connected",0,2); LCD.refresh(); dis = btc.openDataInputStream(); dos = btc.openDataOutputStream(); public static void Disconnect() throws Exception if(btc!=null)WriteBytes(new byte(byte)255,(byte)255,(byte)255);Thread.sleep(100);dos.close();dis.close();btc.close(); publi

16、c 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 byte255; int length = btc.read(buffer, buffer.length); if(length=-2) /lost data, re-sync btc.read(null, 255); ret

17、urn 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 RotateBottomSide(int nQuarter) throws Exception paw.setSpeed(400);int nFixAngle = BaseRotateFix * ( nQuarter > 0 ? 1 :

18、-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 armpublic static void

19、RotateBottom(int nQuarter)throws Exception bottom.rotate(nQuarter * BaseOneQuarter);/Rotate the whole cube from pawpublic static void RotatePaw()throws Exception int nPawHoldPosition = PawHoldPosition - 8;if(HasReducer) nPawHoldPosition = -3 * nPawHoldPosition;int nPawTurnOverPosition = PawTurnOverP

20、osition;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 basepublic stat

21、ic void FixBasePosition() throws Exceptionint 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(

22、realtimeLight < minLight)minLight = realtimeLight;angle = 0;elseangle += step;bottom.rotate(- angle/2 - FixBasePositionOffset);/Read the light difference between light on and light offprivate static int ReadLightDifference() throws Exceptionint l1 = 0, l2 = 0;l1 = light.readValue();light.setFlood

23、light(true);Thread.sleep(20);l2 = light.readValue();light.setFloodlight(false);return l1-l2;/Fix color sensor positionpublic static void FixColorSensorPosition() throws Exceptionint tolerance = 5;ColorMotorBaseAngle = -25;monitor.rotateTo(ColorMotorBaseAngle);Thread.sleep(100);monitor.setSpeed(50);i

24、nt 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

25、= 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 sequenceint nSideIndex=0;ReadOneSide(nSideIndex+);if(CheckCubeReady() RotatePaw();ReadOneSide(nSideInde

26、x+);if(CheckCubeReady() RotatePaw();ReadOneSide(nSideIndex+);if(CheckCubeReady() RotatePaw();ReadOneSide(nSideIndex+);RotateBottom(-1);if(CheckCubeReady() RotatePaw();ReadOneSide(nSideIndex+);if(CheckCubeReady() RotatePaw();if(CheckCubeReady() RotatePaw();ReadOneSide(nSideIndex+);/Read one side by t

27、he 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,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);bot

28、tom.setSpeed(1000);/Read Center Colormonitor.rotateTo(ColorMotorBaseAngle - ColorMotorOffset1);Thread.sleep(delay);SendColorToPC(idx2nSideIndex, idxnSideIndexi+);/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);monito