// 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 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 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
}
/** This function is called periodically during operator control */
public void teleopInit() {
armpid.setInputRange(-180, 180);
armpid.setOutputRange(-1, 1);
armpid.setSetpoint(-22);
}