FUZZY CONTROLLER
// File: ControladorRefuso.java
// Date:
// Description:
// Author:
// Modifications:
// You may need to add other webots classes such as
import com.cyberbotics.webots.controller.DistanceSensor;
import com.cyberbotics.webots.controller.Motor;
import com.cyberbotics.webots.controller.Robot;
import com.cyberbotics.webots.controller.Camera;
import com.cyberbotics.webots.controller.LED;
import net.sourceforge.jFuzzyLogic.FIS;
import net.sourceforge.jFuzzyLogic.plot.JDialogFis;
import com.cyberbotics.webots.controller.Camera;
import com.cyberbotics.webots.controller.GPS;
// Here is the main class of your controller.
// This class defines how to initialize and how to run your controller.
public class ControladorRefuso{
//public void enable(int samplingPeriod);
//public void disable();
//public int getSamplingPeriod();
// This is the main function of your controller.
// It creates an instance of your Robot instance and
// it uses its function(s).
// Note that only one instance of Robot should be created in
// a controller program.
// The arguments of the main function can be specified by the
// "controllerArgs" field of the Robot node
public static void main(String[] args) {
// create the Robot instance.
Robot robot = new Robot();
// get the time step of the current world.
int timeStep = (int) Math.round(robot.getBasicTimeStep());
// Como acceder a los motores del Robot
String nombresMotores[]=new String[]{"motordd","motorpd","motordi","motorpi"};
Motor motores[] = new Motor[nombresMotores.length];
// Como hacer que los motores se queden en una posicion inicial
for(int i=0;i<motores.length;i++){
motores[i] = robot.getMotor(nombresMotores[i]);
motores[i].setPosition(Double.POSITIVE_INFINITY);
motores[i].setVelocity(3.0);
}
Camera camara = robot.getCamera("camera");
camara.enable(timeStep);
GPS gps = robot.getGPS("gps");
gps.enable(timeStep);
LED led = robot.getLED("led");
DistanceSensor sensori = robot.getDistanceSensor("sensori");
DistanceSensor sensord = robot.getDistanceSensor("sensord");
DistanceSensor sensoric = robot.getDistanceSensor("sensoric");
DistanceSensor sensordc = robot.getDistanceSensor("sensordc");
sensori.enable(timeStep);
sensord.enable(timeStep);
sensoric.enable(timeStep);
sensordc.enable(timeStep);
double di = 0.0;
double dd = 0.0;
double dic = 0.0;
double ddc = 0.0;
int luz = 5;
// Si se requiere hacer una pausa de 1 segundo y ejecutar algun codigo
//robot.step(1000);
//System.out.println("Hola");
// Ejemplo para que robot gire hacia la izquierda
/*
motores[0].setVelocity(-3.0);
motores[1].setVelocity(-3.0);
motores[2].setVelocity(3.0);
motores[3].setVelocity(3.0);
*/
// CONTROL DIFUSO
FIS _FIS = FIS.load("Controlador.fcl");
_FIS.setVariable("distanciai",0.0);
_FIS.setVariable("distanciad",0.0);
_FIS.evaluate();
double vel = _FIS.getVariable("velocidad").getLatestDefuzzifiedValue();
System.out.println("Vel: "+vel);
JDialogFis dialogoF = new JDialogFis(_FIS);
dialogoF.setVisible(true);
// You should insert a getDevice-like function in order to get the
// instance of a device of the robot. Something like:
// Motor motor = robot.getMotor("motorname");
// DistanceSensor ds = robot.getDistanceSensor("dsname");
// ds.enable(timeStep);
// Main loop:
// - perform simulation steps until Webots is stopping the controller
while (robot.step(timeStep) != -1) {
// Read the sensors:
// Enter here functions to read sensor data, like:
// double val = ds.getValue();
// Process sensor data here.
// Enter here functions to send actuator commands, like:
// motor.setPosition(10.0);
di = sensori.getValue();
dd = sensord.getValue();
dic = sensori.getValue();
ddc = sensord.getValue();
_FIS.setVariable("distanciai",di);
_FIS.setVariable("distanciad",dd);
_FIS.evaluate();
vel = _FIS.getVariable("velocidad").getLatestDefuzzifiedValue();
System.out.println(di+"||ORIGINAL"+dd+"||"+"||"+ddc+"||"+"||"+dic+"||"+vel);
led.set(0);
dialogoF.repaint();
if(vel <= 2){
for(int i=0;i<motores.length;i++){
if(i<2){
motores[i].setVelocity(-1*vel);
}else{
robot.step(500);
motores[i].setVelocity(vel);
}
}
led.set(1);
}else{
if (dd <= 250 || di <= 250) {
robot.step(100);
if(dd < 100 || di < 100 || dic < 150 || ddc < 150){
//dialogoF.repaint();
_FIS.setVariable("distanciai", 110);
_FIS.setVariable("distanciad", 110);
_FIS.evaluate();
vel = _FIS.getVariable("velocidad").getLatestDefuzzifiedValue();
motores[0].setVelocity(-1.2*vel);
motores[1].setVelocity(-1.2*vel);
motores[2].setVelocity(vel);
motores[3].setVelocity(vel);
led.set(1);
} else if(dd > di){
//dialogoF.repaint();
//robot.step(50);
motores[0].setVelocity(-1*vel);
motores[1].setVelocity(-1*vel);
motores[2].setVelocity(1.1*vel);
motores[3].setVelocity(1.1*vel);
led.set(1);
} else if(di > dd){
//dialogoF.repaint();
//robot.step(50);
motores[0].setVelocity(1.1*vel);
motores[1].setVelocity(1.1*vel);
motores[2].setVelocity(-1*vel);
motores[3].setVelocity(-1*vel);
led.set(1);
}
if (di < 20 || dd < 20 || ddc < 20 || dic < 20){
//dialogoF.repaint();
for(int i=0;i<motores.length;i++){
motores[i].setVelocity(0);
led.set(1);
}
System.out.println("|||||||||||||||||||||||||||||");
}
}else{
//dialogoF.repaint();
for(int i=0;i<motores.length;i++){
motores[i].setVelocity(1.2*vel);
}
led.set(0);
}
}
System.out.println("||GPS X||"+gps.getValues()[0]);
System.out.println("||GPS Y||"+gps.getValues()[1]);
System.out.println("||GPS Z||"+gps.getValues()[2]);
System.out.println("|||||||||||||||||||||||||||||||");
};
// Enter here exit cleanup code.
}
}
Comentarios
Publicar un comentario