Sonar.java 3.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177
  1. import java.io.DataInputStream;
  2. import java.io.DataOutputStream;
  3. import java.io.IOException;
  4. import lejos.nxt.Motor;
  5. import lejos.nxt.NXTRegulatedMotor;
  6. import lejos.nxt.SensorPort;
  7. import lejos.nxt.Sound;
  8. import lejos.nxt.UltrasonicSensor;
  9. import lejos.nxt.comm.BTConnection;
  10. import lejos.nxt.comm.Bluetooth;
  11. import lejos.robotics.navigation.DifferentialPilot;
  12. import lejos.robotics.navigation.Pose;
  13. import lejos.robotics.localization.*;
  14. class Scanner extends Thread {
  15. DataOutputStream output;
  16. OdometryPoseProvider provider;
  17. boolean run;
  18. public boolean scann;
  19. public int increment = 5;
  20. int position = 0;
  21. Scanner (DataOutputStream output, OdometryPoseProvider provider) {
  22. super();
  23. this.output = output;
  24. this.provider = provider;
  25. run = true;
  26. scann = false;
  27. }
  28. public void stop () {
  29. run = false;
  30. }
  31. public void run() {
  32. NXTRegulatedMotor scannerm = Motor.C;
  33. UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S1) ;
  34. while (run) {
  35. if (scann == false) {
  36. position = 0;
  37. scannerm.rotateTo(0);
  38. scannerm.waitComplete();
  39. Thread.yield();
  40. continue;
  41. }
  42. scannerm.rotateTo(position*5);
  43. System.out.println(position);
  44. float distance = sensor.getDistance();
  45. Pose pose = provider.getPose();
  46. try {
  47. output.write('@');
  48. output.write(position);
  49. output.writeFloat(distance);
  50. output.flush();
  51. } catch (IOException e) {
  52. }
  53. position += increment;
  54. if (position == 90 || position == -90)
  55. increment *= -1;
  56. }
  57. scannerm.rotateTo(0);
  58. scannerm.waitComplete();
  59. }
  60. }
  61. public class Sonar {
  62. private static final byte FORWARD = 0;
  63. private static final byte STOP = 1;
  64. private static final byte EXIT = 2;
  65. private static final byte LEFT = 3;
  66. private static final byte RIGHT = 4;
  67. private static final byte BACKWARD = 5;
  68. public static final byte STOPSCANN = 6;
  69. public static final byte STARTSCANN = 7;
  70. public static final byte MOVE = 8;
  71. public static final byte ROTATE = 9;
  72. public static final byte SETPOSE = 10;
  73. public static final byte SETSCANANGLE = 11;
  74. public static void main(String[] args) throws Exception {
  75. BTConnection btc = Bluetooth.waitForConnection();
  76. //USBConnection btc = USB.waitForConnection();
  77. DataInputStream input = btc.openDataInputStream();
  78. DataOutputStream output = btc.openDataOutputStream();
  79. DifferentialPilot pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A, false);
  80. OdometryPoseProvider provider = new OdometryPoseProvider(pilot);
  81. pilot.setRotateSpeed(80);
  82. pilot.setTravelSpeed(20);
  83. Scanner scan = new Scanner(output, provider);
  84. scan.start();
  85. int input_byte;
  86. boolean run = true;
  87. Sound.twoBeeps();
  88. while (run) {
  89. if (input.available() <= 0) {
  90. Thread.yield();
  91. continue;
  92. }
  93. input_byte = input.readByte();
  94. System.out.println(input_byte);
  95. switch (input_byte) {
  96. case FORWARD:
  97. pilot.forward();
  98. break;
  99. case STOP:
  100. pilot.stop();
  101. break;
  102. case EXIT:
  103. run = false;
  104. break;
  105. case LEFT:
  106. pilot.rotateLeft();
  107. break;
  108. case RIGHT:
  109. pilot.rotateRight();
  110. break;
  111. case BACKWARD:
  112. pilot.backward();
  113. break;
  114. case STOPSCANN:
  115. scan.scann = false;
  116. break;
  117. case STARTSCANN:
  118. scan.position = 90;
  119. scan.increment = -Math.abs(scan.increment);
  120. scan.scann = true;
  121. break;
  122. case MOVE:
  123. float d = input.readFloat();
  124. pilot.travel(d);
  125. break;
  126. case ROTATE:
  127. float r = input.readFloat();
  128. pilot.rotate(r);
  129. break;
  130. case SETPOSE:
  131. float x = input.readFloat();
  132. float y = input.readFloat();
  133. float a = input.readFloat();
  134. System.out.println("x: "+x);
  135. System.out.println("y: "+y);
  136. System.out.println("a: "+a);
  137. provider.setPose(new Pose(x,y,a));
  138. break;
  139. case SETSCANANGLE:
  140. int i = input.readByte();
  141. System.out.println("ang: "+i);
  142. scan.increment = i;
  143. break;
  144. }
  145. }
  146. Sound.beep();
  147. scan.stop();
  148. scan.join();
  149. }
  150. }