Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- import java.io.IOException;
- import lejos.navigation.CompassPilot;
- import lejos.nxt.*;
- //import lejos.navigation.*;
- public class MCLcar {
- public static void main (String args []) throws IOException, InterruptedException {
- carBT bt = new carBT();
- LightSensor ls1 = new LightSensor(SensorPort.S1);
- LightSensor ls2 = new LightSensor(SensorPort.S2);
- LightSensor ls3 = new LightSensor(SensorPort.S3);
- final float wheelDiameter = 56; //mm
- final float trackWidth = 111; //mm
- CompassPilot myCompassPilot = new CompassPilot(SensorPort.S4, wheelDiameter, trackWidth, Motor.B, Motor.A, false);
- myOrders order = new myOrders();
- int localDirection;
- int line;
- myCompassPilot.calibrate();
- // Creating bt connection:
- LCD.clear();
- LCD.drawString("Enter to connect",0,0);
- LCD.refresh();
- while(! Button.ENTER.isPressed())
- {}
- bt.connect();
- // Put the map the right way.
- LCD.clear();
- LCD.drawString("Car face norht",0,0);
- LCD.drawString("on the map",0,1);
- LCD.drawString("and press ENTER",0,2);
- LCD.refresh();
- while(! Button.ENTER.isPressed())
- {}
- Thread.sleep(500); // break before next;
- while(! Button.ENTER.isPressed())
- {
- LCD.clear();
- LCD.drawString("Turn map and car",0,0);
- LCD.drawString("so angle is 0.", 0, 1);
- drawData("angle:", myCompassPilot.getAngle(), 5);
- Thread.sleep(300);
- }
- LCD.clear();
- LCD.drawString("Ready for orders.",0,0);
- while (! Button.ESCAPE.isPressed())
- {
- // Receive orders with bluetooth
- try {
- order = bt.receiveData();
- } catch (IOException e) {
- //e.printStackTrace();
- }
- // Clear display
- line = 0;
- LCD.clear();
- // write orderdata to display:
- drawData("Order T:", order.getTheta(), line++);
- drawData("Order D:", order.getDistance(), line++);
- // Turn
- localDirection = order.getTheta();
- if(localDirection != myCompassPilot.getAngle())
- myCompassPilot.rotateTo(localDirection, false); //rotates to the specified angle/direction.
- // write new direction to LCD
- localDirection = myCompassPilot.getAngle();
- drawData("Compass:", localDirection, line);
- line++;
- // Reset Tacho counters / Distance
- myCompassPilot.resetTachoCount();
- // Drive strait forward
- myCompassPilot.travel(order.getDistance(), false);
- Thread.sleep(10); // ms
- // Write on the display.
- drawData("Distance", (int)myCompassPilot.getTravelDistance(), line);
- drawData("LS1:", ls1.readValue(), line+1);
- drawData("LS2:", ls2.readValue(), line+2);
- drawData("LS3:", ls3.readValue(), line+3);
- // Direction according to north
- localDirection = myCompassPilot.getAngle();
- //sends back measurements:
- drawData("Distance:", (int)myCompassPilot.getTravelDistance(), line+4);
- try {
- bt.sendData((int)-(Math.sin(myCompassPilot.getAngle()*Math.PI/180)*myCompassPilot.getTravelDistance()),
- (int)-(Math.cos(myCompassPilot.getAngle()*Math.PI/180)*myCompassPilot.getTravelDistance()),
- localDirection,
- ls1.readValue(), ls2.readValue(), ls3.readValue());
- } catch (IOException e) {
- //e.printStackTrace();
- }
- }
- }
- private static void drawData(String s, int i, int LCDrow)
- {
- LCD.drawString(s,0, LCDrow);
- LCD.drawInt(i,s.length()+1, LCDrow);
- LCD.refresh();
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement