X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fsubsystems%2FDriveTrain.java;h=203078ccfd7f88d9103b8a6ddf4ac529b6212c4c;hb=8bc0fed2615c9a6a51d308e8dfe2d78b14ff2d05;hp=d212059b1c5b91770ae1a0ec6265f403f2672d00;hpb=643241dab07791483b0b8d33b38362c065d31cf3;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 d212059b..203078cc 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,38 +16,26 @@ import edu.wpi.first.wpilibj.RobotDrive; import edu.wpi.first.wpilibj.command.PIDSubsystem; public class DriveTrain extends PIDSubsystem { - /* - * A setpoint is the value we want the PID controller to attempt to adjust the - * system to - * In other words, If we want to drive the robot 4 meters, the setpoint would - * be 4 meters - */ - - // 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 + public DriveTrain() { - super(EP, EI, ED); + super(Constants.DriveTrain.kp, Constants.DriveTrain.ki, + Constants.DriveTrain.kd); frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT); frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT); @@ -54,6 +43,8 @@ public class DriveTrain extends PIDSubsystem { rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT); robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight); + + lidar = new Lidar(I2C.Port.kMXP); leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A, Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X); rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A, @@ -112,6 +103,10 @@ public class DriveTrain extends PIDSubsystem { rightEncoder.reset(); } + public double getLidarDistance() { + return lidar.pidGet(); + } + public double getRightSpeed() { return rightEncoder.getRate(); // in inches per second } @@ -171,9 +166,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(EP, EI, ED); + 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() { @@ -199,10 +196,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 @@ -214,9 +211,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 * EP / 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; } @@ -231,10 +227,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) @@ -249,15 +244,14 @@ 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); + robotDrive.tankDrive(-left, -right); else robotDrive.tankDrive(right, left); } @@ -277,30 +271,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(); @@ -308,10 +301,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) { @@ -330,16 +323,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(); @@ -366,4 +359,5 @@ public class DriveTrain extends PIDSubsystem { leftGearPiston.set(gear); rightGearPiston.set(gear); } + }