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));
}
static {
// stealerEncoder.setDistancePerPulse(.0245);
stealerEncoder.setMaxPeriod(1 /* seconds */);
stealerEncoder.reset();
lassoCounter.reset();
controller.setOutputRange(-0.2, 0.2);
controller.setAbsoluteTolerance(50);
}
private boolean onTarget() {
if (leftPID.getError() < 5.0 / 12 && rightPID.getError() < 5.0 / 12) {
doneCount++;
return true;
} else {
doneCount = 0;
return false;
}
}
public boolean elevatePIDTuning(double height, double speed) {
elevatorPID.setOutputRange(-0.5, speed);
elevatorPID.setSetpoint(height);
elevatorPID.enable();
if (elevatorPID.onTarget()) {
elevatorPID.disable();
robot.elevator.elevatorVics.set(0);
return true;
} else {
return false;
}
}
public boolean elevatePID(double height) {
elevatorPID.setOutputRange(-1, 1);
elevatorPID.setSetpoint(-height);
elevatorPID.enable();
if (elevatorPID.onTarget()) {
elevatorPID.disable();
robot.elevator.elevatorSingle.set(0);
return true;
} else {
return false;
}
}
protected void execute() {
double delta = timer.get();
distance += (drivetrain.getAverageForwardVelocity() * delta);
timer.reset();
// PID tuning code
pidControllerDistance.setPID(
pidConfigDistance.getP(P),
pidConfigDistance.getI(I),
pidConfigDistance.getD(D),
pidConfigDistance.getF(F));
pidConfigDistance.setSetpoint(pidControllerDistance.getSetpoint());
pidConfigDistance.setValue(drivetrainRotation.pidGet());
}
public Balancer(Drivetrain robot) {
this.robot = robot;
sensor = robot.getGyro();
// set up the pid controller - input to pid is the gyro degrees, output is motor PWM signal
controller = new PIDController(P, I, D, sensor, robot);
controller.setTolerance(
tolerance); // must set these values so that the pid has a frame of reference.
controller.setInputRange(-maxDeg, maxDeg);
controller.setOutputRange(-1, 1);
// register with config file
Configuration.getInstance().register("Balancer", this);
}
public void teleopPeriodic() {
armpid.setPID(
SmartDashboard.getNumber("kP"),
SmartDashboard.getNumber("kI"),
SmartDashboard.getNumber("kD"));
if (joy1.getRawButton(1)) {
armpid.enable();
} else {
armpid.disable();
arm.set(0);
}
SmartDashboard.putBoolean("Checkbox 2", limit.get());
System.out.println("limit " + limit.get());
}
public ElevatorPID(Robot robot) {
this.robot = robot;
elevatorPot = new AnalogPotentiometer(Constants.elevatorPot);
elevatorPID = new PIDController(p, i, d, this, robot.elevator.elevatorSingle);
elevatorPID.setAbsoluteTolerance(Constants.elevatorPIDTolerance);
}
public void autonomousInit() {
try {
robot.arm.moveTo(PegPosition.RESET);
} catch (CANTimeoutException e1) {
// TODO Auto-generated catch block
e1.printStackTrace();
}
robot.compressor.start();
Timer.delay(3.5);
robot.grabber.tiltDown();
Timer.delay(2);
robot.grabber.grab();
robot.lineSensor.setLinePreference(LinePreference.LEFT);
controller.enable();
Timer.delay(2.5);
robot.grabber.tiltUp();
Timer.delay(2.5);
try {
robot.arm.moveTo(PegPosition.MIDDLE_OFFSET);
} catch (CANTimeoutException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
public void update(String param, String[] values) {
if (param.equalsIgnoreCase("PID=")) {
if (values.length >= 3) {
P = Double.parseDouble(values[0]);
I = Double.parseDouble(values[1]);
D = Double.parseDouble(values[2]);
}
controller.setPID(P, I, D);
} else if (param.equalsIgnoreCase("tolerance=")) {
if (values.length >= 1) {
tolerance = Double.parseDouble(values[0]);
}
controller.setTolerance(tolerance);
}
}
protected boolean isFinished() {
if (drivebase.onTarget() || isTimedOut()) {
// Disable PID controller
m_pc.disable();
return true;
} else return false;
}
public void robotInit() {
j1 = new Joystick(1);
j2 = new Joystick(0);
shooter = new Victor(2);
shooter2 = new Victor(4);
intake = new Victor(3);
right = new Victor(0);
left = new Victor(1);
rd = new RobotDrive(left, right);
light = new DigitalInput(3);
count = 0;
comp = new Compressor();
inup = new DoubleSolenoid(0, 1);
inup2 = new DoubleSolenoid(2, 3);
gyro = new AnalogGyro(0);
gyro.setSensitivity(.007);
gyro.calibrate();
gyro.reset();
enc = new Encoder(1, 2, true, Encoder.EncodingType.k4X);
enc.setDistancePerPulse(.11977); // circumference of wheel/200 (PPR)
enc.setPIDSourceType(PIDSourceType.kDisplacement);
enc.reset();
out =
new PIDOutput() {
@Override
public void pidWrite(double out) {
rd.setLeftRightMotorOutputs(out, out);
}
};
controller = new PIDController(.007, 0, 0, enc, out);
controller.setAbsoluteTolerance(.25);
controller.disable();
gcontroller = new PIDController(kp, ki, kd, gyro, out);
gcontroller.setAbsoluteTolerance(ktol);
gcontroller.disable();
timerStart = -1;
timeToCancel = -1;
}
public DriveStraight(double distance) {
requires(Robot.drivetrain);
pid =
new PIDController(
4,
0,
0,
new PIDSource() {
public double pidGet() {
return Robot.drivetrain.getDistance();
}
},
new PIDOutput() {
public void pidWrite(double d) {
Robot.drivetrain.drive(d, d);
}
});
pid.setAbsoluteTolerance(0.01);
pid.setSetpoint(distance);
}
/**
* Uses the PID controller that is enabled/disabled in the main class to move to an upright
* position
*/
public void moveToUprightPos() {
vm.autoUpright = true;
stopJawPistons();
neckControl.setSetpoint(vm.JAW_UPRIGHT_POS);
// activate autoneck control if not within 5 ticks of upright position
if (!(sFunctions.getNeckPot() == vm.JAW_UPRIGHT_POS)) {
vm.autoUpright = true;
} else {
vm.autoUpright = false;
}
}
protected void initialize() {
// Use standard mecanum drive
drivebase.initMecanum(false);
/*
* Enable drivebase rotational PID
*/
// Set target angle
m_pc.setSetpoint(m_target);
// Throttle down the max allowed speed
m_pc.setOutputRange(-0.5, 0.5);
// Set tolerance around the target angle
m_pc.setAbsoluteTolerance(5);
// Reset and enable PID controller
m_pc.reset();
m_pc.enable();
}
public DriveTrain() {
// super supplies the PID constants (.05, 0, 0)
super("DriveTrain", .0070, .0003, 0);
robotDrive = new RobotDrive(_leftMaster, _rightMaster);
// Invert the appropriate controllers
// robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
_leftSlave1.changeControlMode(TalonControlMode.Follower);
_leftSlave1.set(_leftMaster.getDeviceID());
_leftSlave2.changeControlMode(TalonControlMode.Follower);
_leftSlave2.set(_leftMaster.getDeviceID());
_rightSlave1.changeControlMode(TalonControlMode.Follower);
_rightSlave1.set(_rightMaster.getDeviceID());
_rightSlave2.changeControlMode(TalonControlMode.Follower);
_rightSlave2.set(_rightMaster.getDeviceID());
ahrs = Robot.ahrs;
// getPIDController() returns the PID Controller that is included with
// classes that extend PIDSubsystems.
turnController = getPIDController();
turnController.disable();
turnController.setInputRange(-180, 180);
turnController.setOutputRange(-.7, .7);
// begin vision test
// frame = RobotMap.FRAME;
// session = RobotMap.CAMERA_SESSION;
// NIVision.IMAQdxConfigureGrab(session);
// colorTable = new NIVision.RawData();
// end vision test
}
/** Runs a pickup routine to get the ball from behind */
public void backPickUp() {
vm.currentNeckSetPoint = vm.BACK_LOAD_POS;
neckControl.setSetpoint(vm.currentNeckSetPoint);
// Spin the rollers to swallow the ball
moveRollerReverse();
/* keep picking up until the ball is detected by the ultra sound
* after which, roll only enough to move the ball to a good position
* and finaly close on the ball and stop the pickup routine
*/
// if (sFunctions.isBallOnUltraSound()) {
// if (pickupEndTimer > 30) {
// pickupEndTimer = 0;
// turnRollerOff();
// vm.pickingUp = false;
// } else {
// pickupEndTimer++;
// }
// }
}
// 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 void autonomousContinuous() {
long currentTime = System.currentTimeMillis();
long timeSinceLastT = currentTime - lastTtime;
// If we're not at a T, continue the loop
if (!robot.lineSensor.isAtT()) return;
System.out.println("At a T");
lastTtime = currentTime;
// If we're seeing the same T, continue the loop
if (timeSinceLastT < T_WAIT_TIME) return;
tCount++;
// If this isn't the last T, continue the loop
if (tCount != NUM_TS) return;
controller.disable();
robot.grabber.release();
// Arm goes down
}
public boolean elevatePIDLock(double height) {
if (robot.elevator.locked) {
robot.elevator.elevatorAutoRelease();
}
elevatorPID.setOutputRange(-1, 1);
elevatorPID.setSetpoint(height);
elevatorPID.enable();
if (elevatorPID.onTarget()) {
elevatorPID.disable();
robot.elevator.elevatorVics.set(0);
if (height > 0) {
robot.elevator.elevatorLock();
}
}
return elevatorPID.onTarget();
}
public static void setSetpoint(double position) {
controller.setSetpoint(position);
}
public static void pidDisable() {
controller.disable();
}
public static void pidEnable() {
controller.enable();
}
public static boolean onTarget() {
return controller.onTarget();
}
public static double getPIDLastOutput() {
return controller.get();
}
public static double getError() {
return controller.getError();
}
public void disabledInit() {
controller.disable();
}
// Called once after isFinished returns true
protected void end() {
// Stop PID and the wheels
pid.disable();
Robot.drivetrain.drive(0, 0);
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return pid.onTarget();
}
