X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fsubsystems%2FDriveTrain.java;h=b7264f1c8bba799dfe8aa9f717e28564e28175ca;hb=9ca89e45fa84b2ec93bc6adf60c7dde1e0a7defb;hp=bf294383c3e3a9c828d1142f8964648153040eba;hpb=a4e99721c66bf58635eae0a089ddb26c1c237c07;p=3501%2F2017steamworks diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index bf29438..b7264f1 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -1,8 +1,8 @@ package org.usfirst.frc.team3501.robot.subsystems; import org.usfirst.frc.team3501.robot.Constants; +import org.usfirst.frc.team3501.robot.MathLib; import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive; -import org.usfirst.frc.team3501.robot.utils.BNO055; import com.ctre.CANTalon; @@ -10,32 +10,39 @@ import edu.wpi.first.wpilibj.ADXRS450_Gyro; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.I2C.Port; import edu.wpi.first.wpilibj.RobotDrive; import edu.wpi.first.wpilibj.command.Subsystem; public class DriveTrain extends Subsystem { - public static double driveP = 0.006, driveI = 0.001, driveD = -0.002; - public static double defaultGyroP = 0.004, defaultGyroI = 0.0013, - defaultGyroD = -0.005; - public static double driveP = 0.008, driveI = 0.001, driveD = -0.002; - public static double defaultGyroP = 0.009, defaultGyroI = 0.00000, - defaultGyroD = -0.000; - private double gyroZero = 0; - - public static final double WHEEL_DIAMETER = 6; // inches - public static final int ENCODER_PULSES_PER_REVOLUTION = 256; + public static double driveP = 0.01, driveI = 0.00115, driveD = -0.002; + public static double smallTurnP = 0.004, smallTurnI = 0.0013, + smallTurnD = 0.005; + public static double largeTurnP = .003, largeTurnI = .0012, largeTurnD = .006; + public static double driveStraightGyroP = 0.01; + + public static final double WHEEL_DIAMETER = 4; // inches + public static final double ENCODER_PULSES_PER_REVOLUTION = 256; public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI / ENCODER_PULSES_PER_REVOLUTION; + public static final double MAINTAIN_CLIMBED_POSITION = 0; + public static final double TIME_TO_CLIMB_FOR = 0; + public static final double CLIMBER_SPEED = 0; + + public static final boolean DRIVE_BRAKE_MODE = true; + public static final boolean DRIVE_COAST_MODE = false; + private static DriveTrain driveTrain; + private final CANTalon frontLeft, frontRight, rearLeft, rearRight; private final RobotDrive robotDrive; private final Encoder leftEncoder, rightEncoder; - private final DoubleSolenoid leftGearPiston, rightGearPiston; + private final DoubleSolenoid leftDriveTrainPiston, rightDriveTrainPiston; + private final DoubleSolenoid gearManipulatorPiston; private ADXRS450_Gyro imu; - private BNO055 imu; + + public boolean shouldBeClimbing = false; private DriveTrain() { // MOTOR CONTROLLERS @@ -57,21 +64,21 @@ public class DriveTrain extends Subsystem { robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight); this.imu = new ADXRS450_Gyro(Constants.DriveTrain.GYRO_PORT); - shifter = DoubleSolenoid(10, Constants.DriveTrain.SHIFTER_FORWARD, - Constants.DriveTrain.SHIFTER_REVERSE); - leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_NUMBER, - Constants.DriveTrain.LEFT_FORWARD, Constants.DriveTrain.LEFT_REVERSE); - rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_NUMBER, - Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE); - this.imu = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS, - BNO055.vector_type_t.VECTOR_EULER, Port.kOnboard, (byte) 0x28); - gyroZero = imu.getHeading(); + // TODO: Not sure if MODULE_NUMBER should be the same for both + leftDriveTrainPiston = new DoubleSolenoid( + Constants.DriveTrain.PISTON_MODULE, + Constants.DriveTrain.LEFT_GEAR_PISTON_FORWARD, + Constants.DriveTrain.LEFT_GEAR_PISTON_REVERSE); + rightDriveTrainPiston = new DoubleSolenoid( + Constants.DriveTrain.PISTON_MODULE, + Constants.DriveTrain.RIGHT_GEAR_PISTON_FORWARD, + Constants.DriveTrain.RIGHT_GEAR_PISTON_REVERSE); - leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_NUMBER, - Constants.DriveTrain.LEFT_FORWARD, Constants.DriveTrain.LEFT_REVERSE); - rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_NUMBER, - Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE); + gearManipulatorPiston = new DoubleSolenoid( + Constants.DriveTrain.PISTON_MODULE, + Constants.DriveTrain.GEAR_MANIPULATOR_PISTON_FORWARD, + Constants.DriveTrain.GEAR_MANIPULATOR_PISTON_REVERSE); } public static DriveTrain getDriveTrain() { @@ -82,7 +89,10 @@ public class DriveTrain extends Subsystem { } // DRIVE METHODS - public void setMotorValues(final double left, final double right) { + public void setMotorValues(double left, double right) { + left = MathLib.restrictToRange(left, -1.0, 1.0); + right = MathLib.restrictToRange(right, -1.0, 1.0); + frontLeft.set(left); rearLeft.set(left); @@ -91,7 +101,10 @@ public class DriveTrain extends Subsystem { } public void joystickDrive(final double thrust, final double twist) { - robotDrive.arcadeDrive(thrust, twist, true); + if ((thrust < 0.1 && thrust > -0.1) && (twist < 0.1 && twist > -0.1)) + robotDrive.arcadeDrive(0, 0, true); + else + robotDrive.arcadeDrive(thrust, twist, true); } public void stop() { @@ -117,11 +130,9 @@ public class DriveTrain extends Subsystem { } public void printEncoderOutput() { - // System.out.println("left: " + getLeftEncoderDistance()); - // System.out.println("right: " + getRightEncoderDistance()); - System.out.println(getAvgEncoderDistance()); System.out.println("left: " + getLeftEncoderDistance()); System.out.println("right: " + getRightEncoderDistance()); + // System.out.println(getAvgEncoderDistance()); } public double getAvgEncoderDistance() { @@ -147,63 +158,61 @@ public class DriveTrain extends Subsystem { // ------Gyro------// public double getAngle() { - return this.imu.getAngle() - this.gyroZero; + return this.imu.getAngle(); } public void resetGyro() { this.imu.reset(); - - public double getAngle() { - if (!this.imu.isInitialized()) - return -1; - return this.imu.getHeading() - this.gyroZero; - } - - public void resetGyro() { - this.gyroZero = this.getAngle(); - - } - - public double getZeroAngle() { - return this.gyroZero; } /* * @return a value that is the current setpoint for the piston kReverse or * KForward */ - public Value getLeftGearPistonValue() { - return leftGearPiston.get(); + public Value getLeftDriveTrainPiston() { + return leftDriveTrainPiston.get(); } /* - * @return a value that is the current setppoint for the piston kReverse or - * kForward + * @return a value that is the current setpoint for the piston kReverse or + * KForward */ - public Value getRightGearPistonValue() { - return rightGearPiston.get(); + public Value getRightDriveTrainPiston() { + return rightDriveTrainPiston.get(); } /* * Changes the ball shift gear assembly to high */ public void setHighGear() { - changeGear(Constants.DriveTrain.HIGH_GEAR); + changeGear(Constants.DriveTrain.FORWARD_PISTON_VALUE); } /* * Changes the ball shift gear assembly to low */ public void setLowGear() { - changeGear(Constants.DriveTrain.LOW_GEAR); + changeGear(Constants.DriveTrain.REVERSE_PISTON_VALUE); } /* * Changes the gear to a DoubleSolenoid.Value */ private void changeGear(DoubleSolenoid.Value gear) { - leftGearPiston.set(gear); - rightGearPiston.set(gear); + leftDriveTrainPiston.set(gear); + rightDriveTrainPiston.set(gear); + } + + public Value getGearManipulatorPistonValue() { + return gearManipulatorPiston.get(); + } + + public void extendGearManipulatorPiston() { + gearManipulatorPiston.set(Constants.DriveTrain.FORWARD_PISTON_VALUE); + } + + public void retractGearManipulatorPiston() { + gearManipulatorPiston.set(Constants.DriveTrain.REVERSE_PISTON_VALUE); } @Override @@ -211,4 +220,11 @@ public class DriveTrain extends Subsystem { setDefaultCommand(new JoystickDrive()); } + public void setCANTalonsBrakeMode(boolean mode) { + frontLeft.enableBrakeMode(mode); + rearLeft.enableBrakeMode(mode); + + frontRight.enableBrakeMode(mode); + rearRight.enableBrakeMode(mode); + } }