/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package com.test.linearrobot;
import ca.teamdave.caruso.CarusoException;
import ca.teamdave.caruso.Controller;
import ca.teamdave.caruso.Machine;
import ca.teamdave.caruso.actuator.DoubleActuator;
import ca.teamdave.caruso.config.ConfigTable;
import ca.teamdave.caruso.sensor.DoubleSensor;
import ca.teamdave.predicdave.PredictiveLinearController;
/**
*
* @author leigh
*/
public class LinearRobotController implements Controller {
private DoubleSensor robotPosition;
private DoubleActuator robotOutput;
private DoubleActuator targetPos;
private DoubleActuator targetSpeed;
private DoubleActuator targetAccel;
private PredictiveLinearController controller;
boolean firstCycle;
boolean done;
ConfigTable config;
public void robotInit(Machine machine) throws CarusoException {
this.robotPosition = new DoubleSensor(machine.getSensor(LrNames.POSITION_SENSOR));
this.robotOutput = new DoubleActuator(machine.getActuator(LrNames.OUTPUT_FORCE));
this.targetPos = new DoubleActuator(machine.getActuator(LrNames.DESIRED_POSITION));
this.targetSpeed = new DoubleActuator(machine.getActuator(LrNames.DESIRED_SPEED));
this.targetAccel = new DoubleActuator(machine.getActuator(LrNames.DESIRED_ACCEL));
this.config = machine.getConfig();
this.controller = new PredictiveLinearController(
config.getDouble(LrNames.PID_P),
config.getDouble(LrNames.PID_I),
config.getDouble(LrNames.PID_D),
config.getDouble(LrNames.V_FF),
config.getDouble(LrNames.A_FF),
config.getDouble(LrNames.D_FF),
config.getDouble(LrNames.CYCLE_TIME),
config.getDouble(LrNames.PID_EPS));
this.firstCycle = true;
this.done = false;
}
public void periodicUpdate() throws CarusoException {
if (this.firstCycle){
this.controller.setDestination(0.0, 0.0, 10.0, 0.0,
config.getDouble(LrNames.DECCEL_MAX),
config.getDouble(LrNames.ACCEL_MAX),
config.getDouble(LrNames.SPEED_MAX));
this.firstCycle = false;
}
double output = this.controller.updateCycle(this.robotPosition.get());
this.robotOutput.set(output);
this.targetPos.set(this.controller.getTargetPosition());
this.targetSpeed.set(this.controller.getTargetSpeed());
this.targetAccel.set(this.controller.getTargetAccel());
}
}