MyRobotClass

package nl.vu.cs.s2.simbadtest;

import java.awt.Color;
import java.awt.image.BufferedImage;
import java.util.ArrayList;

import javax.vecmath.Vector3d;
import javax.vecmath.Point3d;
import javax.vecmath.Color3f;
import simbad.sim.Agent;
import simbad.sim.CameraSensor;
import simbad.sim.RangeSensorBelt;
import simbad.sim.RobotFactory;
import simbad.sim.LampActuator;



public class myRobot extends Agent{

    CameraSensor camera;
    BufferedImage cameraImage;
    RangeSensorBelt sonars;
    RangeSensorBelt bumpers;
    LampActuator lamp;
    ArrayList<Point3d> posArray = new ArrayList<Point3d>();

    public myRobot(Vector3d position, String name, Color3f color) {
        super(position, name);
        this.setColor(color);
        // Add bumpers
        bumpers = RobotFactory.addBumperBeltSensor(this, 16);
        // Add sonars
        sonars = RobotFactory.addSonarBeltSensor(this, 8);

        camera = RobotFactory.addCameraSensor(this);

        lamp = 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);
    }
    public boolean boxDetected(){
        cameraImage = camera.createCompatibleImage();
        camera.copyVisionImage(cameraImage);
        Color pixelRGB = new Color(cameraImage.getRGB(50,50));
        int red = pixelRGB.getRed();
        int green = pixelRGB.getGreen();
        int blue = pixelRGB.getBlue();
        if (red > 0 && green == 0 && blue == 0){
            return true;
        }
        return false;

    }
    //general strategy to avoid obstacles
    public void avoidObstacle(double l, double fl, double f, double fr, double r){
//      System.out.println("avoidObstacle");
        if((fl < fr) && fl < 0.5){
            rotateY(-5);
        } else if ((fr < fl)  && fr < 0.5) {
            rotateY(5);
        } else if((fr > f) && f < 0.5 && (fl > f)){
            rotateY(-5);
        } else if (l < 0.3){
            rotateY(-5);
        } else if (r < 0.3){
            rotateY(5);
        }

    }

    // locate the box in the space
    public void pinpoint(){
        // TODO add error & classify with list
        boolean contains = false;
        Point3d coordinates = new Point3d();
        getCoords(coordinates);
        coordinates.x = Math.round(coordinates.x);
        coordinates.y = Math.round(coordinates.y);
        coordinates.z = Math.round(coordinates.z);
        for(int i=0;i<posArray.size();i++){
            if((coordinates.x == posArray.get(i).x) && (coordinates.z == posArray.get(i).z)){
                contains = true;
            }
        }
        if(!contains){
            posArray.add(coordinates);
        }
    }

    /** This method is call cyclically (20 times per second)
     *  by the simulator engine. */
    public void performBehavior() {

        setTranslationalVelocity(0.5);


        if(getCounter() % 500 == 0){
//          print the list of elements
            System.out.println("-------------------begin of coords-----------------");
            for(int i=0; i<posArray.size();i++){
                System.out.println(posArray.get(i));
            }
            System.out.println("-------------------end of coords-----------------");
        }
        // perform the following actions every 5 virtual seconds
        if(getCounter() % 5 == 0) {

            double frontSonar = sonars.getMeasurement(0);
            double frontLeftSonar = sonars.getMeasurement(5);
            double leftSonar = sonars.getMeasurement(4);
            double rightSonar = sonars.getMeasurement(2);
            double frontRightSonar = sonars.getMeasurement(1);

            if(bumpers.oneHasHit()){
                rotateY(-45);
            }
            if(boxDetected()){
                setRotationalVelocity(0);
                if(frontSonar < 0.3){
                    pinpoint();
                    avoidObstacle(leftSonar, frontLeftSonar, frontSonar, frontRightSonar, rightSonar);
                }
            }
            else if((sonars.getFrontQuadrantHits() > 0) || rightSonar < 0.5 || leftSonar < 0.5){
                avoidObstacle(leftSonar, frontLeftSonar, frontSonar, frontRightSonar, rightSonar);
            }
        }
        //frequently change orientation
        if ((getCounter() % 50) == 0) {
            if(!boxDetected()){
                setRotationalVelocity(Math.PI / 2 * (0.5 - Math.random()));
            }
        }
    }
}