+
+ /*
+ * 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() {
+ return getAvgEncoderDistance();
+ }
+
+ public void joystickDrive(double left, double right) {
+ int type = Constants.DriveTrain.DRIVE_TYPE;
+
+ // Handle flipping of the "front" of the robot
+ double k = (isFlipped() ? -1 : 1);
+
+ robotDrive.tankDrive(-left, -right);
+
+ // if (type == Constants.DriveTrain.TANK) {
+ // // TODO Test this for negation and for voltage vs. [-1,1] outputs
+ // double leftSpeed = (-frontLeft.get() + -rearLeft.get()) / 2;
+ // double rightSpeed = (-frontRight.get() + -rearRight.get()) / 2;
+ // left = ((Constants.DriveTrain.kADJUST - 1) * leftSpeed + left)
+ // / Constants.DriveTrain.kADJUST;
+ // right = ((Constants.DriveTrain.kADJUST - 1) * rightSpeed + right)
+ // / Constants.DriveTrain.kADJUST;
+ // robotDrive.tankDrive(-left * k, -right * k);
+ // } else if (type == Constants.DriveTrain.ARCADE) {
+ // double speed = (-frontLeft.get() + -rearLeft.get() + -frontRight.get() +
+ // -rearRight
+ // .get()) / 2;
+ // left = ((Constants.DriveTrain.kADJUST - 1) * speed + left)
+ // / Constants.DriveTrain.kADJUST;
+ // robotDrive.arcadeDrive(left * k, right);
+ // }
+ }
+
+ public void setMotorSpeeds(double left, double right) {
+ double k = (isFlipped() ? -1 : 1);
+ robotDrive.tankDrive(-left, -right);
+ }
+
+ /**
+ * @return a value that is the current setpoint for the piston (kReverse or
+ * kForward)
+ */
+ public Value getGearPistonValue() {
+ return leftGearPiston.get(); // Pistons should always be in the same state
+ }
+
+ /**
+ * Changes the ball shift gear assembly to high
+ */
+ public void setHighGear() {
+ changeGear(Constants.DriveTrain.HIGH_GEAR);
+ }
+
+ /**
+ * Changes the ball shift gear assembly to low
+ */
+ public void setLowGear() {
+ changeGear(Constants.DriveTrain.LOW_GEAR);
+ }
+
+ /**
+ * Changes the gear to a DoubleSolenoid.Value
+ */
+ public void changeGear(DoubleSolenoid.Value gear) {
+ leftGearPiston.set(gear);
+ rightGearPiston.set(gear);
+ }
+
+ /**
+ * Switches drivetrain gears from high to low or low to high
+ */
+ public void switchGear() {
+ Value currentValue = getGearPistonValue();
+ Value setValue = (currentValue == Constants.DriveTrain.HIGH_GEAR) ? Constants.DriveTrain.LOW_GEAR
+ : Constants.DriveTrain.HIGH_GEAR;
+ changeGear(setValue);
+ }
+
+ /**
+ * Toggle whether the motor outputs are flipped, effectively switching which
+ * side of the robot is the front.
+ */
+ public void toggleFlipped() {
+ outputFlipped = !outputFlipped;
+ }
+
+ public boolean isFlipped() {
+ return outputFlipped;
+ }
+