/**
* @param wheelDiameter
* @param gearRatio
* @param threshold Amount of allowable error in number of encoder ticks to destination.
* @param slowDownStart Number of wheel revolutions before slowing down the first time.
* @param fineTuneStart Number of wheel revolutions before slowing down the second time.
* @param typePID Determines conversion factor (degrees to ticks or distance to ticks based off
* type)
* @param motors
*/
public EncoderPIDController(
double wheelDiameter,
int gearRatio,
double threshold,
double slowDownStart,
double fineTuneStart,
TypePID typePID,
DcMotor... motors) {
super(threshold, motors);
boolean isSymmetrical = (motors.length % 2 == 0);
int sides = 1;
if (isSymmetrical) {
sides = 2;
}
super.setSides(sides);
this.wheelDiameter = wheelDiameter;
this.gearRatio = gearRatio;
this.slowDownStart = slowDownStart;
this.fineTuneStart = fineTuneStart;
this.typePID = typePID;
switch (typePID) {
case DRIVE:
super.setConversionFactor(TICKS_PER_REVOLUTION / (wheelDiameter * Math.PI * gearRatio));
Log.d("SetTargets", "" + conversionFactor);
break;
case LIFT:
break;
default:
break;
}
targets = new double[sides];
}
public void goToPosition(String sPosition) {
m_sPosition = sPosition;
pidLeft.setSetpoint(m_preferences.getFloat(sPosition, 0));
pidRight.setSetpoint(m_preferences.getFloat(sPosition + "R", 0));
System.out.println(sPosition + " Setpoint = " + m_preferences.getFloat(sPosition, 0));
}
public void navigateRobot() {
System.out.println("Path Index: " + pathIndex);
System.out.println("goalPath Size: " + goalPath.size());
System.out.println("locX, locY, locTheta" + locX + ", " + locY + ", " + locTheta);
System.out.println("goalX, goalY, goalTheta: " + goalX + ", " + goalY + ", " + goalTheta);
if (goalPath.size() > 0 && !done) {
if (pathIndex < goalPath.size() - 1
&& Math.abs(goalX - locX) < 0.2
&& Math.abs(goalY - locY) < 0.2) {
pathIndex++;
}
if (pathIndex == goalPath.size() - 1
&& Math.abs(goalX - locX) < 0.05
&& Math.abs(goalY - locY) < 0.05) {
pathIndex++;
}
if (pathIndex >= goalPath.size()) {
done = true;
setVelocities(0.0, 0.0);
}
goalX = goalPath.get(pathIndex).x;
goalY = goalPath.get(pathIndex).y;
goalTheta = Math.atan2(goalY - locY, goalX - locX);
if (goalTheta < 0) {
goalTheta += 2 * Math.PI;
}
double error = goalTheta - locTheta;
if (error > Math.PI) {
error -= 2 * Math.PI;
} else if (error < -Math.PI) {
error += 2 * Math.PI;
}
if (Math.abs(error) < Math.PI / 8.0) {
setVelocities(0.3, anglePID.update(error));
} else {
setVelocities(0, anglePID.update(error));
}
// setVelocities(0.2, anglePID.update(error));
}
}
// Initialize your subsystem here
public LoaderClimberOld2PID() {
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
// super("LoaderClimberOld2PID", 1.5, 0.1, 0.0);
// setAbsoluteTolerance(0.02);
// getPIDController().setContinuous(false);
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
m_preferences = Preferences.getInstance();
if (leftJag != null) { // for practice robot
// Fix for motors going in opposite directios
PIDOutput rightOutput =
new PIDOutput() {
public void pidWrite(double output) {
rightJag.pidWrite(-1 * output);
}
};
pidLeft = new PIDController(P, I, D, leftAngleCh, leftJag); // PID Output Interface
pidRight = new PIDController(P, I, D, rightAngleCh, rightOutput); // PID Output Interface*/
LiveWindow.addActuator("LoaderClimber", "PIDLeft Angle Controller", pidLeft);
LiveWindow.addActuator("LoaderClimber", "PIDLeft Angle Controller", pidRight);
pidLeft.setSetpoint(m_preferences.getFloat("Retract", 2.4f));
pidRight.setSetpoint(m_preferences.getFloat("RetractR", 2.4f));
pidLeft.setInputRange(0, 4);
pidRight.setInputRange(0, 4);
pidLeft.setPercentTolerance(PID_TOL);
pidRight.setPercentTolerance(PID_TOL);
pidLeft.enable(); // - Enables the PID controller.
pidRight.enable(); // - Enables the PID controller.
// disable();
}
}
public boolean anglegood() {
return pidLeft.onTarget() && pidRight.onTarget();
}
