import com.ctre.CANTalon;
+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 final CANTalon frontLeft, frontRight, rearLeft, rearRight;
private final RobotDrive robotDrive;
private final Encoder leftEncoder, rightEncoder;
+ private final DoubleSolenoid leftGearPiston, rightGearPiston;
+ private ADXRS450_Gyro imu;
private BNO055 imu;
private DriveTrain() {
// ROBOT DRIVE
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();
+
+ 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);
}
public static DriveTrain getDriveTrain() {
setMotorValues(0, 0);
}
- public double getFrontLeftMotorVal() {
- return frontLeft.get();
- }
-
- public double getFrontRightMotorVal() {
- return frontRight.get();
- }
-
- public double getRearLeftMotorVal() {
- return frontLeft.get();
+ public double getLeftMotorVal() {
+ return (frontLeft.get() + rearLeft.get()) / 2;
}
- public double getRearRightMotorVal() {
- return frontLeft.get();
+ public double getRightMotorVal() {
+ return (frontRight.get() + rearRight.get()) / 2;
}
// ENCODER METHODS
}
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());
}
}
// ------Gyro------//
+ public double getAngle() {
+ return this.imu.getAngle() - this.gyroZero;
+ }
+
+ public void resetGyro() {
+ this.imu.reset();
+
public double getAngle() {
if (!this.imu.isInitialized())
return -1;
return this.gyroZero;
}
+ /*
+ * @return a value that is the current setpoint for the piston kReverse or
+ * KForward
+ */
+ public Value getLeftGearPistonValue() {
+ return leftGearPiston.get();
+ }
+
+ /*
+ * @return a value that is the current setpoint for the piston kReverse or
+ * KForward
+ */
+ public Value getRightGearPistonValue() {
+ return rightGearPiston.get();
+ }
+
+ /*
+ * 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
+ */
+ private void changeGear(DoubleSolenoid.Value gear) {
+ leftGearPiston.set(gear);
+ rightGearPiston.set(gear);
+ }
+
@Override
protected void initDefaultCommand() {
setDefaultCommand(new JoystickDrive());