X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fsubsystems%2FDriveTrain.java;h=96f1d1150fd5d87a2d5e5531f8515fcf10302e4f;hb=7e360ef5fdf5bb9ae932367bbeb5a930f54ee732;hp=be0103cf2c81b99add42e14ec12fda7504865854;hpb=7a4df3c5063f7a9229223a0e76e536494c7b3271;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 be0103cf..96f1d115 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -1,9 +1,10 @@ package org.usfirst.frc.team3501.robot.subsystems; import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.GyroLib; import org.usfirst.frc.team3501.robot.MathLib; import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive; +import org.usfirst.frc.team3501.robot.sensors.GyroLib; +import org.usfirst.frc.team3501.robot.sensors.Lidar; import edu.wpi.first.wpilibj.CANTalon; import edu.wpi.first.wpilibj.CounterBase.EncodingType; @@ -15,45 +16,26 @@ import edu.wpi.first.wpilibj.RobotDrive; import edu.wpi.first.wpilibj.command.PIDSubsystem; public class DriveTrain extends PIDSubsystem { - // Encoder PID Proportional Constants P, I, and D - private static double EP = 0.013, EI = 0.000015, ED = -0.002; - - // Gyro PID Constants P, I, and D - private static double GP = 0.018, GI = 0.000015, GD = 0; - private static double pidOutput = 0; - - // PID Controller tolerances for the error - private static double encoderTolerance = 8.0, gyroTolerance = 5.0; - // Current Drive Mode Default Drive Mode is Manual private int DRIVE_MODE = 1; - - // Different Drive Modes - private static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3; + private static double pidOutput = 0; private Encoder leftEncoder, rightEncoder; + + public static Lidar lidar; + private CANTalon frontLeft, frontRight, rearLeft, rearRight; private RobotDrive robotDrive; private GyroLib gyro; 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() { - super(kp, ki, kd); + super(Constants.DriveTrain.kp, Constants.DriveTrain.ki, + Constants.DriveTrain.kd); frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT); frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT); @@ -61,6 +43,9 @@ public class DriveTrain extends PIDSubsystem { rearRight = new CANTalon(Constants.DriveTrain.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, @@ -82,6 +67,7 @@ public class DriveTrain extends PIDSubsystem { Constants.DriveTrain.LEFT_REVERSE); rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE); + Constants.DriveTrain.inverted = false; } @Override @@ -118,6 +104,10 @@ public class DriveTrain extends PIDSubsystem { rightEncoder.reset(); } + public double getLidarDistance() { + return lidar.pidGet(); + } + public double getRightSpeed() { return rightEncoder.getRate(); // in inches per second } @@ -177,9 +167,11 @@ public class DriveTrain extends PIDSubsystem { // Updates the PID constants based on which control mode is being used public void updatePID() { if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) - this.getPIDController().setPID(kp, ki, kd); + this.getPIDController().setPID(Constants.DriveTrain.kp, + Constants.DriveTrain.ki, Constants.DriveTrain.kd); else - this.getPIDController().setPID(gp, gd, gi); + this.getPIDController().setPID(Constants.DriveTrain.gp, + Constants.DriveTrain.gd, Constants.DriveTrain.gi); } public CANTalon getFrontLeft() { @@ -205,10 +197,10 @@ public class DriveTrain extends PIDSubsystem { /* * 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 @@ -220,9 +212,8 @@ public class DriveTrain extends PIDSubsystem { if (Math.abs(output) > 0 && Math.abs(output) < 0.3) output = Math.signum(output) * 0.3; left = output; - right = output + drift * kp / 10; - } - else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) { + right = output + drift * Constants.DriveTrain.kp / 10; + } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) { left = output; right = -output; } @@ -237,10 +228,9 @@ public class DriveTrain extends PIDSubsystem { /* * 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 + * + * @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() { if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) @@ -255,13 +245,16 @@ public class DriveTrain extends PIDSubsystem { /* * @param left and right setpoints to set to the left and right side of tank * inverted is for Logan, wants the robot to invert all controls left = right - * and right = left - * negative input is required for the regular rotation because RobotDrive - * tankdrive method drives inverted + * and right = left negative input is required for the regular rotation + * because RobotDrive tankdrive method drives inverted */ public void drive(double left, double right) { robotDrive.tankDrive(-left, -right); // dunno why but inverted drive (- values is forward) + if (!Constants.DriveTrain.inverted) + robotDrive.tankDrive(-left, -right); + else + robotDrive.tankDrive(right, left); } /* @@ -279,30 +272,29 @@ public class DriveTrain extends PIDSubsystem { } /* - * Sets the encoder mode - * Updates the PID constants sets the tolerance and sets output/input ranges - * Enables the PID controllers + * Sets the encoder mode Updates the PID constants sets the tolerance and sets + * output/input ranges Enables the PID controllers */ public void setEncoderPID() { DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE; this.updatePID(); - this.setAbsoluteTolerance(encoderTolerance); + this.setAbsoluteTolerance(Constants.DriveTrain.encoderTolerance); this.setOutputRange(-1.0, 1.0); this.setInputRange(-200.0, 200.0); this.enable(); } /* - * Sets the Gyro Mode - * Updates the PID constants, sets the tolerance and sets output/input ranges - * Enables the PID controllers + * Sets the Gyro Mode Updates the PID constants, sets the tolerance and sets + * output/input ranges Enables the PID controllers */ private void setGyroPID() { DRIVE_MODE = Constants.DriveTrain.GYRO_MODE; this.updatePID(); - this.getPIDController().setPID(gp, gi, gd); + this.getPIDController().setPID(Constants.DriveTrain.gp, + Constants.DriveTrain.gi, Constants.DriveTrain.gd); - this.setAbsoluteTolerance(gyroTolerance); + this.setAbsoluteTolerance(Constants.DriveTrain.gyroTolerance); this.setOutputRange(-1.0, 1.0); this.setInputRange(-360.0, 360.0); this.enable(); @@ -310,10 +302,10 @@ public class DriveTrain extends PIDSubsystem { /* * Turning method that should be used repeatedly in a command - * + * * First constrains the angle to within -360 and 360 since that is as much as * we need to turn - * + * * Configures Gyro PID and sets the setpoint as an angle */ public void turnAngle(double angle) { @@ -332,16 +324,16 @@ public class DriveTrain extends PIDSubsystem { } /* - * @return a value that is the current setpoint for the piston - * kReverse or kForward + * @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 + * @return a value that is the current setpoint for the piston kReverse or + * kForward */ public Value getRightGearPistonValue() { return rightGearPiston.get();