Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- package nl.vu.cs.s2.simbadtest;
- import javax.imageio.ImageIO;
- import javax.vecmath.Vector3d;
- import java.io.*;
- import simbad.sim.Agent;
- import simbad.sim.CameraSensor;
- import simbad.sim.LightSensor;
- import simbad.sim.RangeSensorBelt;
- import simbad.sim.RobotFactory;
- import javax.imageio.ImageIO;
- import java.io.IOException;
- import java.awt.Image;
- import java.awt.image.BufferedImage;
- public class ExampleRobot extends Agent {
- RangeSensorBelt sonars;
- LightSensor lightSensor;
- CameraSensor cameraSensor;
- BufferedImage vision = new BufferedImage(100,100,BufferedImage.TYPE_INT_RGB);
- BufferedImage cropped = new BufferedImage(1,1,BufferedImage.TYPE_INT_RGB);
- public ExampleRobot(Vector3d position, String name) {
- super(position, name);
- //Add camera
- cameraSensor = RobotFactory.addCameraSensor(this);
- cameraSensor.setUpdatePerSecond(20);
- //Add bumper
- RobotFactory.addBumperBeltSensor(this, 8);
- //Add sonar
- sonars = new RangeSensorBelt(0, 0, 10, 4, 0, 0);
- this.addSensorDevice(sonars, new Vector3d(0, 0, 0) , 0);
- sonars.setUpdatePerSecond(20);
- //Add light sensor
- lightSensor = RobotFactory.addLightSensor(this);
- lightSensor.setUpdatePerSecond(20);
- RobotFactory.addLamp(this);
- }
- /** This method is called by the simulator engine on reset. */
- public void initBehavior() {
- System.out.println("I exist and my name is " + this.name);
- }
- // prints rgb values of pixels
- public void printPixelARGB(int pixel) {
- int alpha = (pixel >> 24) & 0xff;
- int red = (pixel >> 16) & 0xff;
- int green = (pixel >> 8) & 0xff;
- int blue = (pixel) & 0xff;
- System.out.println("argb: " + alpha + ", " + red + ", " + green + ", " + blue);
- }
- /** This method is call cyclically (20 times per second) by the simulator engine. */
- public void performBehavior() {
- cameraSensor.copyVisionImage(vision);
- try {
- //takes scneen shot, crops central pixel
- ImageIO.write(vision, "jpg", new File("/Users/jakub/Downloads/out.jpg"));
- cropped = vision.getSubimage(50, 50, 1, 1);
- ImageIO.write(cropped, "jpg", new File("/Users/jakub/Downloads/out2.jpg"));
- int pixel = cropped.getRGB(0, 0);
- int green = (pixel >> 8) & 0xff;
- System.out.print(green);
- //checks is pixel is green (goal) when reaches goal stop bot
- if(green > 150){
- this.setTranslationalVelocity(0);
- setRotationalVelocity(0);
- }else{
- //if touching the wall, just turn right
- if(this.collisionDetected()){
- this.setTranslationalVelocity(0);
- setRotationalVelocity(-2*Math.PI);
- }else{
- //If there is obstacle ahead turn right
- if(sonars.getMeasurement(0)<=2){
- this.setTranslationalVelocity(1);
- setRotationalVelocity(-Math.PI/4);
- }
- //If there is no wall ahead ...
- else{
- //and there is no wall on the left turn turn left
- if(sonars.getMeasurement(1)>=2){
- this.setTranslationalVelocity(1);
- setRotationalVelocity(Math.PI/4);
- }
- //and there is wall on the left turn left
- else{
- //navigation on smaller distance to keep bot close to the wall
- //turn left
- if((sonars.getMeasurement(1)>=1)){
- System.out.println("5");
- this.setTranslationalVelocity(1);
- setRotationalVelocity(Math.PI/4);
- }
- //turn right
- else{
- System.out.println("6");
- this.setTranslationalVelocity(1);
- setRotationalVelocity(-Math.PI/4);
- }
- }
- }
- }
- }
- } catch (IOException e) {
- // TODO Auto-generated catch block
- e.printStackTrace();
- }
- //System.out.println(this.v1);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement