+ public double getGyroAngle() {
+ return gyro.getRotationZ().getAngle();
+ }
+
+ public void resetGyro() {
+ gyro.reset();
+ }
+
+ public void printEncoder(int i, int n) {
+ if (i % n == 0) {
+ System.out.println("Left: " + this.getLeftDistance());
+ System.out.println("Right: " + this.getRightDistance());
+
+ }
+ }
+
+ public void printGyroOutput() {
+ System.out.println("Gyro Angle" + -this.getGyroAngle());
+ }
+
+ /*
+ * returns the PID output that is returned by the PID Controller
+ */
+ public double getOutput() {
+ return pidOutput;
+ }
+
+ // Updates the PID constants based on which control mode is being used
+ public void updatePID() {
+ if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
+ this.getPIDController().setPID(Constants.DriveTrain.kp,
+ Constants.DriveTrain.ki, Constants.DriveTrain.kd);
+ else
+ this.getPIDController().setPID(Constants.DriveTrain.gp,
+ Constants.DriveTrain.gd, Constants.DriveTrain.gi);
+ }
+
+ public CANTalon getFrontLeft() {
+ return frontLeft;
+ }
+
+ public CANTalon getFrontRight() {
+ return frontRight;
+ }
+
+ public CANTalon getRearLeft() {
+ return rearLeft;
+ }
+
+ public CANTalon getRearRight() {
+ return rearRight;
+ }
+
+ public int getMode() {
+ return DRIVE_MODE;
+ }
+
+ /*
+ * Method is a required method that the PID Subsystem uses to return the
+ * calculated PID value to the driver
+ *
+ * @param Gives the user the output from the PID algorithm that is calculated
+ * internally
+ *
+ * Body: Uses the output, does some filtering and drives the robot
+ */
+ @Override
+ protected void usePIDOutput(double output) {
+ double left = 0;
+ double right = 0;
+ if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) {
+ double drift = this.getLeftDistance() - this.getRightDistance();
+ if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
+ output = Math.signum(output) * 0.3;
+ left = output;
+ right = output + drift * Constants.DriveTrain.kp / 10;
+ } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
+ left = output;
+ right = -output;
+ }
+ drive(left, right);
+ pidOutput = output;
+ }
+
+ @Override
+ protected double returnPIDInput() {
+ return sensorFeedback();
+ }
+
+ /*
+ * Checks the drive mode
+ *
+ * @return the current state of the robot in each state
+ * Average distance from both sides of tank drive for Encoder Mode
+ * Angle from the gyro in GYRO_MODE
+ */
+ private double sensorFeedback() {
+ if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
+ return getAvgEncoderDistance();
+ else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE)
+ return -this.getGyroAngle();
+ // counterclockwise is positive on joystick but we want it to be negative
+ else
+ return 0;
+ }
+
+ /*
+ * @param left and right setpoints to set to the left and right side of tank
+ * inverted is for Logan, wants the robot to invert all controls left = right
+ * and right = left
+ * negative input is required for the regular rotation because RobotDrive
+ * tankdrive method drives inverted
+ */
+ public void drive(double left, double right) {
+ robotDrive.tankDrive(-left, -right);
+ // dunno why but inverted drive (- values is forward)
+ if (!Constants.DriveTrain.inverted)
+ robotDrive.tankDrive(-left,
+ -right);
+ else
+ robotDrive.tankDrive(right, left);