/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package ca.teamdave.predicdave;
import ca.teamdave.pid.PidLinear;
import ca.teamdave.predicdave.pathmaking.LinearPathMaker;
import ca.teamdave.predicdave.pathmaking.Path;
/**
* A kinematic prediction based PID controller for linear single dimentional
* feedback systems with PID feedback plus velocity and acceleration feedforward
* control.
*
* You *shouldn't* need to use any 'I' component for this controller, unless
* there is an actual constant force applied to the system, like gravity. A
* version of this system using a high-static friction feedback PID may be
* needed if that is a problem.
*
* @author leigh
*/
public class PredictiveLinearController {
// Feedforward constants
private final double vff, aff, dff;
private final double cycleTime;
// control structures
private PidLinear feedback;
private Path path;
// need an epsilon for when control falls back to straight pid
private double epsilon;
private double curTime;
// keep track of the last moving setpoints for debugging
double targetPosition, targetSpeed, targetAccel;
public PredictiveLinearController(double p, double i, double d, double vff, double aff, double dff, double cycleTime, double epsilon) {
this.feedback = new PidLinear(p, i, d, cycleTime);
this.vff = vff;
this.aff = aff;
this.dff = dff;
this.cycleTime = cycleTime;
this.epsilon = epsilon;
this.curTime = 0.0;
this.path = null;
this.targetPosition = 0.0;
this.targetSpeed = 0.0;
this.targetAccel = 0.0;
}
/**
* Makes the controller construct a new path for the set point
* @param curPos The current position of the system
* @param curSpeed The current velocity of the system
* @param destPos The destination position of the controller
* @param destSpeed The velocity to try to be going at the destination. This
* velocity must be in the same direction as the direction from curPos to
* destPos, or 0
* @param Dmax The maximum decceleration allowed (in sensorUnits/(second^2)
* @param Amax The maximum acceleration allowed (in sensorUnits/(second^2)
* @param Vmax the maximum speed allowed (in sensorUnits/second
*/
public void setDestination(double curPos, double curSpeed, double destPos, double destSpeed, double Dmax, double Amax, double Vmax) {
this.curTime = 0.0;
this.path = LinearPathMaker.createPath(this.curTime, curPos, curSpeed, destPos, destSpeed, Dmax, Amax, Vmax, this.epsilon);
}
/**
* Updates the controller by one cycle
* @param sensorValue The current value of the sensor, in sensible units
* @return the output of the controller for this cycle
*/
public double updateCycle(double sensorValue) {
double output = 0.0;
// get the desired position, speed, and accel from the path
this.targetPosition = this.path.getPosition(this.curTime);
this.targetSpeed = this.path.getSpeed(this.curTime);
this.targetAccel = this.path.getAccel(this.curTime);
this.curTime += this.cycleTime;
this.feedback.setDestination(targetPosition);
output += this.feedback.updateCycle(sensorValue);
// if accelerating
if (targetAccel * targetSpeed >= 0.0) {
output += this.aff * targetAccel;
} else { // if deccelerating
output += this.dff * targetAccel;
}
output += this.vff * targetSpeed;
return output;
}
// output target motion for debugging
public double getTargetPosition() {
return this.targetPosition;
}
// output target motion for debugging
public double getTargetSpeed() {
return this.targetSpeed;
}
// output target motion for debugging
public double getTargetAccel() {
return this.targetAccel;
}
}