Advertisement
Guest User

Untitled

a guest
Feb 21st, 2019
88
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 4.02 KB | None | 0 0
  1. package nl.vu.cs.s2.simbadtest;
  2.  
  3.  
  4. import javax.imageio.ImageIO;
  5. import javax.vecmath.Vector3d;
  6. import java.io.*;
  7.  
  8.  
  9. import simbad.sim.Agent;
  10. import simbad.sim.CameraSensor;
  11. import simbad.sim.LightSensor;
  12. import simbad.sim.RangeSensorBelt;
  13. import simbad.sim.RobotFactory;
  14.  
  15. import javax.imageio.ImageIO;
  16. import java.io.IOException;
  17.  
  18. import java.awt.Image;
  19. import java.awt.image.BufferedImage;
  20.  
  21. public class ExampleRobot extends Agent {
  22.  
  23. RangeSensorBelt sonars;
  24. LightSensor lightSensor;
  25. CameraSensor cameraSensor;
  26. BufferedImage vision = new BufferedImage(100,100,BufferedImage.TYPE_INT_RGB);
  27. BufferedImage cropped = new BufferedImage(1,1,BufferedImage.TYPE_INT_RGB);
  28.  
  29. public ExampleRobot(Vector3d position, String name) {
  30. super(position, name);
  31.  
  32. //Add camera
  33. cameraSensor = RobotFactory.addCameraSensor(this);
  34. cameraSensor.setUpdatePerSecond(20);
  35.  
  36. //Add bumper
  37. RobotFactory.addBumperBeltSensor(this, 8);
  38.  
  39. //Add sonar
  40. sonars = new RangeSensorBelt(0, 0, 10, 4, 0, 0);
  41. this.addSensorDevice(sonars, new Vector3d(0, 0, 0) , 0);
  42. sonars.setUpdatePerSecond(20);
  43.  
  44. //Add light sensor
  45. lightSensor = RobotFactory.addLightSensor(this);
  46. lightSensor.setUpdatePerSecond(20);
  47.  
  48. RobotFactory.addLamp(this);
  49. }
  50.  
  51. /** This method is called by the simulator engine on reset. */
  52. public void initBehavior() {
  53. System.out.println("I exist and my name is " + this.name);
  54. }
  55.  
  56. // prints rgb values of pixels
  57. public void printPixelARGB(int pixel) {
  58. int alpha = (pixel >> 24) & 0xff;
  59. int red = (pixel >> 16) & 0xff;
  60. int green = (pixel >> 8) & 0xff;
  61. int blue = (pixel) & 0xff;
  62. System.out.println("argb: " + alpha + ", " + red + ", " + green + ", " + blue);
  63. }
  64.  
  65. /** This method is call cyclically (20 times per second) by the simulator engine. */
  66. public void performBehavior() {
  67.  
  68. cameraSensor.copyVisionImage(vision);
  69. try {
  70. //takes scneen shot, crops central pixel
  71. ImageIO.write(vision, "jpg", new File("/Users/jakub/Downloads/out.jpg"));
  72. cropped = vision.getSubimage(50, 50, 1, 1);
  73. ImageIO.write(cropped, "jpg", new File("/Users/jakub/Downloads/out2.jpg"));
  74. int pixel = cropped.getRGB(0, 0);
  75. int green = (pixel >> 8) & 0xff;
  76. System.out.print(green);
  77.  
  78. //checks is pixel is green (goal) when reaches goal stop bot
  79. if(green > 150){
  80. this.setTranslationalVelocity(0);
  81. setRotationalVelocity(0);
  82. }else{
  83. //if touching the wall, just turn right
  84. if(this.collisionDetected()){
  85. this.setTranslationalVelocity(0);
  86. setRotationalVelocity(-2*Math.PI);
  87. }else{
  88.  
  89. //If there is obstacle ahead turn right
  90. if(sonars.getMeasurement(0)<=2){
  91. this.setTranslationalVelocity(1);
  92. setRotationalVelocity(-Math.PI/4);
  93. }
  94.  
  95.  
  96.  
  97. //If there is no wall ahead ...
  98. else{
  99.  
  100. //and there is no wall on the left turn turn left
  101. if(sonars.getMeasurement(1)>=2){
  102. this.setTranslationalVelocity(1);
  103. setRotationalVelocity(Math.PI/4);
  104. }
  105.  
  106. //and there is wall on the left turn left
  107. else{
  108. //navigation on smaller distance to keep bot close to the wall
  109. //turn left
  110. if((sonars.getMeasurement(1)>=1)){
  111. System.out.println("5");
  112. this.setTranslationalVelocity(1);
  113. setRotationalVelocity(Math.PI/4);
  114. }
  115. //turn right
  116. else{
  117. System.out.println("6");
  118. this.setTranslationalVelocity(1);
  119. setRotationalVelocity(-Math.PI/4);
  120. }
  121. }
  122. }
  123.  
  124. }
  125. }
  126.  
  127. } catch (IOException e) {
  128. // TODO Auto-generated catch block
  129. e.printStackTrace();
  130. }
  131.  
  132. //System.out.println(this.v1);
  133.  
  134.  
  135.  
  136.  
  137.  
  138.  
  139. }
  140. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement