X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fsubsystems%2FDriveTrain.java;h=1a4e6e5b96c04dfc58689e377aa2ec5ba9dd1ae8;hb=949415d065993cac3fd0c271eb3ba5d6b5ec90af;hp=6c004b42eea16bd0afd9db354925939176b5bd0e;hpb=2c9f5abb5057ebd41a3824c329c6bfcca97a5e83;p=3501%2Fstronghold-2016 diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 6c004b42..1a4e6e5b 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -1,56 +1,91 @@ package org.usfirst.frc.team3501.robot.subsystems; import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.Lidar; +import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive; import edu.wpi.first.wpilibj.CANTalon; import edu.wpi.first.wpilibj.CounterBase.EncodingType; +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; -import edu.wpi.first.wpilibj.PIDController; -import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.RobotDrive; +import edu.wpi.first.wpilibj.command.PIDSubsystem; + +public class DriveTrain extends PIDSubsystem { + // Determines if the "front" of the robot has been reversed + private boolean outputFlipped = false; + + private static double pidOutput = 0; -public class DriveTrain extends Subsystem { - // Drivetrain related objects private Encoder leftEncoder, rightEncoder; - public static Lidar lidar; + private CANTalon frontLeft, frontRight, rearLeft, rearRight; - private PIDController frontLeftC, frontRightC, rearLeftC, rearRightC; + private RobotDrive robotDrive; + + private DoubleSolenoid leftGearPiston, rightGearPiston; + // Drivetrain specific constants that relate to the inches per pulse value for // the encoders - private final static double WHEEL_DIAMETER = 6.0; // in inches - private final static double PULSES_PER_ROTATION = 256; // in pulses - private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches - private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches - public final static double INCHES_PER_PULSE = (((Math.PI) - * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) - / WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER; - - // Drivetrain specific constants that relate to the PID controllers - private final static double Kp = 1.0, Ki = 0.0, - Kd = 0.0 * (OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) - / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER; public DriveTrain() { - frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT); - frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT); - rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT); - rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT); + super(Constants.DriveTrain.kp, Constants.DriveTrain.ki, + Constants.DriveTrain.kd); + + frontLeft = new CANTalon(Constants.DriveTrain.DRIVE_FRONT_LEFT); + frontRight = new CANTalon(Constants.DriveTrain.DRIVE_FRONT_RIGHT); + rearLeft = new CANTalon(Constants.DriveTrain.DRIVE_REAR_LEFT); + rearRight = new CANTalon(Constants.DriveTrain.DRIVE_REAR_RIGHT); + + robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight); - lidar = new Lidar(I2C.Port.kOnboard); leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A, Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X); rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A, Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X); leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE); rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE); - leftEncoder.setDistancePerPulse(INCHES_PER_PULSE); - rightEncoder.setDistancePerPulse(INCHES_PER_PULSE); + leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE); + rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE); + + this.disable(); + + leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_SHIFT_MODULE, + Constants.DriveTrain.LEFT_SHIFT_FORWARD, + Constants.DriveTrain.LEFT_SHIFT_REVERSE); + rightGearPiston = new DoubleSolenoid( + Constants.DriveTrain.RIGHT_SHIFT_MODULE, + Constants.DriveTrain.RIGHT_SHIFT_FORWARD, + Constants.DriveTrain.RIGHT_SHIFT_REVERSE); } @Override protected void initDefaultCommand() { + setDefaultCommand(new JoystickDrive()); + } + + // Print tne PID Output + public void printOutput() { + System.out.println("PIDOutput: " + pidOutput); + } + + private double getAvgEncoderDistance() { + return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2; + } + + // Whether or not the PID Controller thinks we have reached the target + // setpoint + public boolean reachedTarget() { + if (this.onTarget()) { + this.disable(); + return true; + } else { + return false; + } + } + + public void stop() { + setMotorSpeeds(0, 0); } public void resetEncoders() { @@ -58,10 +93,6 @@ public class DriveTrain extends Subsystem { rightEncoder.reset(); } - public double getLidarDistance() { - return lidar.pidGet(); - } - public double getRightSpeed() { return rightEncoder.getRate(); // in inches per second } @@ -82,20 +113,168 @@ public class DriveTrain extends Subsystem { return leftEncoder.getDistance(); // in inches } - public double getDistance() { - return (getRightDistance() + getLeftDistance()) / 2.0; // in inches + // Get error between the setpoint of PID Controller and the current state of + // the robot + public double getError() { + return Math.abs(this.getSetpoint() - getAvgEncoderDistance()); } - public void stop() { - setMotorSpeeds(0, 0); + public void printEncoder(int i, int n) { + if (i % n == 0) { + System.out.println("Left: " + this.getLeftDistance()); + System.out.println("Right: " + this.getRightDistance()); + + } + } + + /* + * 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() { + this.getPIDController().setPID(Constants.DriveTrain.kp, + Constants.DriveTrain.ki, Constants.DriveTrain.kd); + } + + public CANTalon getFrontLeft() { + return frontLeft; + } + + public CANTalon getFrontRight() { + return frontRight; + } + + public CANTalon getRearLeft() { + return rearLeft; + } + + public CANTalon getRearRight() { + return rearRight; } - public void setMotorSpeeds(double leftSpeed, double rightSpeed) { - // speed passed to right motor is negative because right motor rotates in - // opposite direction - this.frontLeft.set(leftSpeed); - this.frontRight.set(-rightSpeed); - this.rearLeft.set(leftSpeed); - this.rearRight.set(-rightSpeed); + /* + * 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; + 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; + setMotorSpeeds(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() { + 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; + } + }