Mediante el muestreo de errores estadísticos (PID) vamos corrigiendo la posición. Si cae hacia delante aceleramos el motor. Si se va hacia atrás, aceleramos el motor hacia atrás.
Para ello, utilizamos tres tipos de errores:
- Error P: más conocido como error proporcional a la posición con respecto al punto de equilibrio.
- Error I: según valla incrementándose el error aumenta su valor
- Error D: más conocido como error derivativo, que compara el estado actual con el anterior
El código utilizado es el siguiente:
import java.util.Vector;
import lejos.nxt.*;
import lejos.robotics.navigation.TachoPilot;
public class principal {
public static LightSensor LS = new LightSensor(SensorPort.S1);
public static int velocidadBase = 1;
public static TachoPilot navigator = new TachoPilot(5.6f, 11.1f, Motor.C, Motor.A);
public static PID pid = new PID();
public static int valormedio;
public static int errorP;
public static int errorD;
public static int errorI;
public static void main(String[] args) {
try{
Thread.sleep(1000);
while (!Button.ENTER.isPressed()) LCD.drawString(LS.readValue()+" ",0 ,0 );
LS.calibrateLow();
while (Button.ENTER.isPressed()) LCD.drawString("calibrando",0 ,0 );
while (!Button.ENTER.isPressed()) LCD.drawString("calibrando",0 ,0 );
LS.calibrateHigh();
LCD.clearDisplay();
while (Button.ENTER.isPressed()) LCD.drawString(LS.readValue()+" ",0 ,0 );
while (!Button.ENTER.isPressed()) LCD.drawString(LS.readValue()+" ",0 ,0 );
valormedio = LS.readValue();
while (Button.ENTER.isPressed()) {
LCD.drawString(LS.readValue()+" ",0 ,0 );
LCD.drawString(valormedio+" ", 0, 1);
}
while (!Button.ENTER.isPressed()) {
LCD.drawString(""+LS.readValue()+" ",0 ,0 );
LCD.drawString(valormedio+" ", 0, 1);
}
for (int i=0; i<20; i++) pid.erAcumulado.addElement(0);
navigator.setSpeed(velocidadBase);
navigator.forward();
LCD.clearDisplay();
LCD.drawString(valormedio+" ", 0, 0);
while(!Button.ESCAPE.isPressed()){
int medida = LS.readValue();
pid.actualizarVector(medida);
errorP = pid.calculaerp(valormedio, medida);
errorD = pid.calculaerd(medida);
errorI = pid.calculaeri();
if (velocidadBase*((float)1.3*errorP + errorD + errorI)>0) navigator.backward();
else navigator.forward();
navigator.setMoveSpeed(velocidadBase*((float)1.3*errorP + errorD + errorI));//*1.3
LCD.drawString(errorP+" ",0,1);
LCD.drawString(errorD+" ",0,2);
LCD.drawString(errorI+" ",0,3);
}
} catch(InterruptedException e){}
}
}
class PID{
public Vector erAcumulado = new Vector(20);
public int calculaerp(int valorMedio, int medida){
return (medida - valorMedio);
}
public int calculaerd(int medida){
return (int)(medida - (Integer)erAcumulado.elementAt(18));
}
public int calculaeri(){
int error = 0;
for (int i=0; i<20; i++){
error += (Integer)erAcumulado.elementAt(i);
}
return (int)(error*0.005)*(int)(error*0.005);
}
public void actualizarVector(int error){
erAcumulado.removeElementAt(0);
erAcumulado.addElement(error);
}
}
Como curiosidad, nuestro robot al usar un sensor de luz no se comporta igual en toda las condiciones. Nuestro robot consiguió permanecer de pie mas de 20 segundos, lo cual supera nuestras expectativas.
