SHARE
TWEET

Henrik Klaus Martin

a guest Jan 16th, 2009 161 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import java.io.IOException;
  2.  
  3.  
  4. import lejos.navigation.CompassPilot;
  5. import lejos.nxt.*;
  6. //import lejos.navigation.*;
  7.  
  8.  
  9. public class MCLcar {
  10.         public static void main (String args []) throws IOException, InterruptedException {
  11.                
  12.                 carBT bt = new carBT();
  13.                
  14.                 LightSensor ls1 = new LightSensor(SensorPort.S1);
  15.                 LightSensor ls2 = new LightSensor(SensorPort.S2);
  16.                 LightSensor ls3 = new LightSensor(SensorPort.S3);
  17.                
  18.                 final float wheelDiameter = 56; //mm
  19.             final float trackWidth = 111; //mm
  20.            
  21.                 CompassPilot myCompassPilot = new CompassPilot(SensorPort.S4, wheelDiameter, trackWidth, Motor.B, Motor.A, false);
  22.                 myOrders order = new myOrders();
  23.                
  24.                 int localDirection;
  25.                 int line;
  26.                
  27.                 myCompassPilot.calibrate();
  28.                
  29.                 // Creating bt connection:
  30.                 LCD.clear();
  31.                 LCD.drawString("Enter to connect",0,0);
  32.                 LCD.refresh(); 
  33.                 while(! Button.ENTER.isPressed())
  34.                 {}
  35.                 bt.connect();
  36.                
  37.                 // Put the map the right way.
  38.                 LCD.clear();
  39.                 LCD.drawString("Car face norht",0,0);
  40.                 LCD.drawString("on the map",0,1);
  41.                 LCD.drawString("and press ENTER",0,2);
  42.                 LCD.refresh();
  43.                 while(! Button.ENTER.isPressed())
  44.                 {}
  45.                
  46.                 Thread.sleep(500); // break before next;
  47.                
  48.                 while(! Button.ENTER.isPressed())
  49.                 {
  50.                         LCD.clear();
  51.                         LCD.drawString("Turn map and car",0,0);
  52.                         LCD.drawString("so angle is 0.", 0, 1);
  53.                         drawData("angle:", myCompassPilot.getAngle(), 5);
  54.                         Thread.sleep(300);
  55.                 }
  56.        
  57.                 LCD.clear();
  58.                 LCD.drawString("Ready for orders.",0,0);
  59.                
  60.         while (! Button.ESCAPE.isPressed())
  61.         {                              
  62.                 // Receive orders with bluetooth
  63.                 try {
  64.                                 order = bt.receiveData();
  65.                 } catch (IOException e) {
  66.                                 //e.printStackTrace();
  67.                         }
  68.                 // Clear display
  69.                 line = 0;
  70.                 LCD.clear();
  71.                 // write orderdata to display:
  72.                         drawData("Order T:", order.getTheta(), line++);
  73.                         drawData("Order D:", order.getDistance(), line++);
  74.                
  75.                 // Turn
  76.                         localDirection = order.getTheta();
  77.                 if(localDirection != myCompassPilot.getAngle())
  78.                         myCompassPilot.rotateTo(localDirection, false);         //rotates to the specified angle/direction.
  79.                 // write new direction to LCD
  80.                 localDirection = myCompassPilot.getAngle();
  81.                 drawData("Compass:", localDirection, line);
  82.                        
  83.                 line++;
  84.                
  85.                 // Reset Tacho counters / Distance
  86.                 myCompassPilot.resetTachoCount();
  87.                
  88.                 // Drive strait forward
  89.                 myCompassPilot.travel(order.getDistance(), false);
  90.  
  91.                 Thread.sleep(10); // ms
  92.  
  93.                 // Write on the display.
  94.                 drawData("Distance", (int)myCompassPilot.getTravelDistance(), line);
  95.                 drawData("LS1:", ls1.readValue(), line+1);
  96.                 drawData("LS2:", ls2.readValue(), line+2);
  97.                 drawData("LS3:", ls3.readValue(), line+3);
  98.                        
  99.                         // Direction according to north
  100.                         localDirection = myCompassPilot.getAngle();    
  101.                         //sends back measurements:
  102.                         drawData("Distance:", (int)myCompassPilot.getTravelDistance(), line+4);
  103.                         try {
  104.                                 bt.sendData((int)-(Math.sin(myCompassPilot.getAngle()*Math.PI/180)*myCompassPilot.getTravelDistance()),
  105.                                                         (int)-(Math.cos(myCompassPilot.getAngle()*Math.PI/180)*myCompassPilot.getTravelDistance()),
  106.                                                         localDirection,
  107.                                                         ls1.readValue(), ls2.readValue(), ls3.readValue());
  108.                                
  109.                         } catch (IOException e) {
  110.                                 //e.printStackTrace();
  111.                         }
  112.         }
  113.         }
  114.        
  115.         private static void drawData(String s, int i, int LCDrow)
  116.     {
  117.         LCD.drawString(s,0, LCDrow);
  118.         LCD.drawInt(i,s.length()+1, LCDrow);
  119.         LCD.refresh();
  120.     }
  121. }
RAW Paste Data
Top