X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;ds=sidebyside;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fsubsystems%2FDriveTrain.java;h=e19b46f02643a6c24af4d7d261ae9f13fb6c74a8;hb=ca325a5866b4f601b171b02ac767393bd996d44a;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..e19b46f0 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,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, @@ -112,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 } @@ -171,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(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() { @@ -214,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 * 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; } @@ -230,11 +227,16 @@ public class DriveTrain extends PIDSubsystem { } /* - * Checks the drive mode + * Checks the drive mode <<<<<<< 9728080f491e9fb09795494349dba1297f447c0f * - * @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 + * ======= + * + * @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 + * >>>>>>> Move all constants in DeadReckoning to Auton class because it makes + * more sense */ private double sensorFeedback() { if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) @@ -249,15 +251,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); } @@ -265,9 +266,9 @@ public class DriveTrain extends PIDSubsystem { /* * constrains the distance to within -100 and 100 since we aren't going to * drive more than 100 inches - * + * * Configure Encoder PID - * + * * Sets the setpoint to the PID subsystem */ public void driveDistance(double dist, double maxTimeOut) { @@ -277,30 +278,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(); @@ -330,16 +330,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 +366,8 @@ public class DriveTrain extends PIDSubsystem { leftGearPiston.set(gear); rightGearPiston.set(gear); } + + public void toggleTimeDeadReckoning() { + Constants.Auton.isUsingTime = !Constants.Auton.isUsingTime; + } }