From: Meryem Esa Date: Thu, 11 Feb 2016 03:37:37 +0000 (-0800) Subject: instead of just one lidar, add one left and one right X-Git-Url: http://challenge-bot.com/repos/?p=3501%2Fstronghold-2016;a=commitdiff_plain;h=9e65c0561274031799dcc9069987d7d0ebe48508 instead of just one lidar, add one left and one right --- diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index d212059b..41a8ac16 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -15,38 +15,41 @@ 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 kp = 0.013, ki = 0.000015, kd = -0.002; + 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 Encoder leftEncoder, rightEncoder; + + public static Lidar leftLidar; + public static Lidar rightLidar; + 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(EP, EI, ED); + super(kp, ki, kd); frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT); frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT); @@ -54,6 +57,10 @@ public class DriveTrain extends PIDSubsystem { rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT); robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight); + + leftLidar = new Lidar(I2C.Port.kOnboard); + rightLidar = new Lidar(I2C.Port.kOnboard); // TODO: find port for second + // lidar leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A, Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X); rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A, @@ -72,10 +79,9 @@ public class DriveTrain extends PIDSubsystem { gyro.start(); leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_FORWARD, - Constants.DriveTrain.LEFT_REVERSE); + +Constants.DriveTrain.LEFT_REVERSE); rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE); - Constants.DriveTrain.inverted = false; } @Override @@ -83,7 +89,6 @@ public class DriveTrain extends PIDSubsystem { setDefaultCommand(new JoystickDrive()); } - // Print tne PID Output public void printOutput() { System.out.println("PIDOutput: " + pidOutput); } @@ -92,8 +97,6 @@ public class DriveTrain extends PIDSubsystem { 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(); @@ -112,6 +115,14 @@ public class DriveTrain extends PIDSubsystem { rightEncoder.reset(); } + public double getLeftLidarDistance() { + return leftLidar.pidGet(); + } + + public double getsRightLidarDistance() { + return rightLidar.pidGet(); + } + public double getRightSpeed() { return rightEncoder.getRate(); // in inches per second } @@ -132,8 +143,6 @@ public class DriveTrain extends PIDSubsystem { return leftEncoder.getDistance(); // in inches } - // Get error between the setpoint of PID Controller and the current state of - // the robot public double getError() { if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) return Math.abs(this.getSetpoint() - getAvgEncoderDistance()); @@ -161,19 +170,15 @@ public class DriveTrain extends PIDSubsystem { System.out.println("Gyro Angle" + -this.getGyroAngle()); } - /* - * 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() { if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) - this.getPIDController().setPID(EP, EI, ED); + this.getPIDController().setPID(kp, ki, kd); else - this.getPIDController().setPID(GP, GD, GI); + this.getPIDController().setPID(gp, gd, gi); } public CANTalon getFrontLeft() { @@ -196,15 +201,6 @@ public class DriveTrain extends PIDSubsystem { return DRIVE_MODE; } - /* - * 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; @@ -214,9 +210,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 * kp / 10; + } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) { left = output; right = -output; } @@ -229,13 +224,6 @@ public class DriveTrain extends PIDSubsystem { 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() { if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) return getAvgEncoderDistance(); @@ -246,41 +234,17 @@ public class DriveTrain extends PIDSubsystem { return 0; } - /* - * @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 - */ 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); } - /* - * 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) { dist = MathLib.constrain(dist, -100, 100); setEncoderPID(); setSetpoint(dist); } - /* - * 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(); @@ -290,15 +254,10 @@ public class DriveTrain extends PIDSubsystem { this.enable(); } - /* - * 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(gp, gi, gd); this.setAbsoluteTolerance(gyroTolerance); this.setOutputRange(-1.0, 1.0); @@ -306,14 +265,6 @@ public class DriveTrain extends PIDSubsystem { this.enable(); } - /* - * 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) { angle = MathLib.constrain(angle, -360, 360); setGyroPID(); @@ -329,39 +280,22 @@ public class DriveTrain extends PIDSubsystem { rearRight.set(right); } - /* - * @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 - */ public void changeGear(DoubleSolenoid.Value gear) { leftGearPiston.set(gear); rightGearPiston.set(gear);