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;
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;
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 rightDriveTrainPiston;
+ // private final Solenoid leftDriveTrainPiston;
+ private final DoubleSolenoid gearManipulatorPiston;
private ADXRS450_Gyro imu;
- private BNO055 imu;
private DriveTrain() {
// MOTOR CONTROLLERS
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 Solenoid(Constants.DriveTrain.PISTON_MODULE,
+ // Constants.DriveTrain.LEFT_GEAR_PISTON_PORT);
+ rightDriveTrainPiston = new DoubleSolenoid(
+ Constants.DriveTrain.PISTON_MODULE,
+ Constants.DriveTrain.DRIVETRAIN_GEAR_FORWARD,
+ Constants.DriveTrain.DRIVETRAIN_GEAR_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() {
}
// 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);
}
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 tankDrive(double left, double right) {
+ robotDrive.tankDrive(left, right);
}
public void stop() {
}
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() {
// ------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 boolean getLeftDriveTrainPiston() {
+ // return leftDriveTrainPiston.get();
+ // }
/*
* @return a value that is the current setpoint for the piston kReverse or
* KForward
*/
- public Value getGearPistonValue() {
- return leftGearPiston.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);
+ System.out.println("shifting to " + gear);
+ rightDriveTrainPiston.set(gear);
+ System.out.println("after: " + this.getRightDriveTrainPiston());
+
+ //
+ // if (gear == Constants.DriveTrain.FORWARD_PISTON_VALUE)
+ // leftDriveTrainPiston.set(Constants.DriveTrain.EXTEND_VALUE);
+ // else
+ // leftDriveTrainPiston.set(Constants.DriveTrain.RETRACT_VALUE);
+ }
+
+ 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
protected void initDefaultCommand() {
setDefaultCommand(new JoystickDrive());
}
-
}
projects / 3501 / 2017steamworks / blobdiff