diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java new file mode 100644 index 0000000..1b1d14b --- /dev/null +++ b/src/main/java/frc/robot/AutoAim.java @@ -0,0 +1,151 @@ +package frc.robot; + +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class AutoAim { + + private double error; + private double motorPower; + private double angleDegrees; + private double gyroDegrees; + + private boolean sweeping; + + public enum AutoAimState { + SET_ANGLES, + AIM, + IDLE; + } + private AutoAimState state; + private Drivetrain drive; + private Limelight limelight; + private Shooter shooter; + private AHRS gyro; + + /** + * Constructor + * + * @param drive drive train object. + * @param limelight limelight object. + * @param shooter shooter object. + * @param gyro gyro object. + */ + public AutoAim(Drivetrain drive, Limelight limelight, Shooter shooter, AHRS gyro) { + state = AutoAimState.IDLE; + this.drive = drive; + this.limelight = limelight; + this.shooter = shooter; + this.gyro = gyro; + } + + /** + * Runs Automatic Aiming state machine. + */ + public void run() { + switch (state) { + + case SET_ANGLES: + SetAngles(); + break; + + case AIM: + AimX(); + AimAngle(); + break; + + case IDLE: + break; + } + + SmartDashboard.putString("AutoAimState", state.toString()); + SmartDashboard.putNumber("MotorPower", motorPower); + SmartDashboard.putNumber("gyroDegrees", gyroDegrees); + } + + /** + * Sets degrees to turn the robot to. + */ + private void SetAngles() { + if (limelight.isTargetVisible()) { + sweeping = false; + drive.stopSweep(); + gyroDegrees = gyro.getYaw() + limelight.getTargetXDegrees(); + state = AutoAimState.AIM; + } else { + if (!sweeping) { + drive.resetState(); + } + sweeping = true; + drive.sweep(); + } + } + + /** + * Aims to robot to the X-coordinate of the power port. + */ + private void AimX() { + // error = (gyroDegrees - gyro.getYaw()) / 29.8; + error = (gyroDegrees - gyro.getYaw()); + // motorPower = -.5 * error; + motorPower = -error * 1; + drive.driveLeft(motorPower); + drive.driveRight(-motorPower); + } + + /** + * Aims the shooter angle to the appropriate angle, based on distance, to the + * power port. + */ + private void AimAngle() { + angleDegrees = 1 * limelight.getTargetVertical(); // TODO: Use a table from testing to create an equation to + // plug this into + shooter.setAngle(angleDegrees); + } + + /** + * Gets if done aiming. + * + * @return true if robot is done aiming, false if not done aiming. + */ + private boolean getDoneAiming() { + if (getWithinTolerance(angleDegrees, shooter.getAngleInDegrees(), 1) && getWithinTolerance(gyroDegrees, gyro.getYaw(), 1)) { + return true; + } else { + return false; + } + } + + /** + * Gets if input is within tolerance of its goal. + * + * @param goal desired value of input. + * @param current current value of input. + * @param tolerance tolerance of current input to desired input. + * @return true if within tolerance, false if not within tolerance. + */ + private boolean getWithinTolerance(double goal, double current, double tolerance) { + return Math.abs(goal - current) < tolerance; + } + + /** + * Resets the state machine. + */ + public void resetState() { + state = AutoAimState.SET_ANGLES; + } + + /** + * Stops the state machine + */ + public void stopState() { + state = AutoAimState.IDLE; + } + + /** + * Gets the aiming state. + */ + public AutoAimState getState() { + return state; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Drivetrain.java b/src/main/java/frc/robot/Drivetrain.java index 19fcdfb..06b3c6a 100644 --- a/src/main/java/frc/robot/Drivetrain.java +++ b/src/main/java/frc/robot/Drivetrain.java @@ -1,5 +1,7 @@ package frc.robot; +import com.kauailabs.navx.frc.AHRS; + public class Drivetrain { /** @@ -8,9 +10,19 @@ public class Drivetrain { */ private static final double DEADBAND_LIMIT = 0.02; + private enum SweepState { + SET_ANGLE, SWEEP_LEFT, SWEEP_RIGHT, IDLE; + } + + private SweepState sweepState; + private DriveModule left; private DriveModule right; private PowercellDetection detector; + private Limelight limelight; + private AHRS gyro; + + private double gyroAngle; /** * Constructor @@ -18,13 +30,17 @@ public class Drivetrain { * @param left The left drive module * @param right The right drive module * @param detector The powercell detection object. + * @param gyro The gyro object. */ - Drivetrain(DriveModule left, DriveModule right, PowercellDetection detector) { + Drivetrain(DriveModule left, DriveModule right, PowercellDetection detector, AHRS gyro) { this.left = left; this.right = right; right.setInverted(true); this.detector = detector; + this.gyro = gyro; + + sweepState = SweepState.IDLE; } /** @@ -90,6 +106,76 @@ public double deadband(double in) { return Math.abs(in) > DEADBAND_LIMIT ? in : 0.0; } + /** + * Runs the sweeping procedure. + */ + public void sweep() { + switch (sweepState) { + + case SET_ANGLE: + sweepAngle(); + break; + + case SWEEP_LEFT: + sweepLeft(); + break; + + case SWEEP_RIGHT: + sweepRight(); + break; + + case IDLE: + break; + } + } + + /** + * Sets home gyro angle. + */ + private void sweepAngle() { + gyroAngle = gyro.getAngle(); + sweepState = SweepState.SWEEP_LEFT; + } + + /** + * Sweeps to the left 80 degrees. + */ + private void sweepLeft() { + if (gyro.getAngle() < gyroAngle + 80) { + driveLeft(-0.25); + driveRight(0.25); + } else { + sweepState = SweepState.SWEEP_RIGHT; + } + } + + /** + * Sweeps to the right 80 degrees. + * + */ + private void sweepRight() { + if (gyro.getAngle() > gyroAngle - 80) { + driveLeft(0.25); + driveRight(-0.25); + } else { + sweepState = SweepState.SWEEP_LEFT; + } + } + + /** + * Resets to the state to setting the angle. + */ + public void resetState() { + sweepState = SweepState.SET_ANGLE; + } + + /** + * Sets the state to idle. + */ + public void stopSweep() { + sweepState = SweepState.IDLE; + } + /** * Moves the robot to intercept powercells * diff --git a/src/main/java/frc/robot/Limelight.java b/src/main/java/frc/robot/Limelight.java index 0b9bc5b..d3dee06 100644 --- a/src/main/java/frc/robot/Limelight.java +++ b/src/main/java/frc/robot/Limelight.java @@ -7,6 +7,10 @@ public class Limelight { + // TODO: Get proper dimension varibles. + private final double IMG_WIDTH = 320; + private final double CAM_FOV = 59.6; + private double tv; private double tx; private double ty; @@ -54,6 +58,8 @@ public double getTargetX() { return this.tx; } + + /** * @return current target y-coordinate. */ @@ -75,6 +81,13 @@ public double getTargetVertical() { return this.tvert; } + /** + * @return current target x-coordinate in degrees. + */ + public double getTargetXDegrees() { + return this.tx*(CAM_FOV/IMG_WIDTH); + } + public void setLightEnabled(boolean enabled) { limelightTable.getEntry("ledMode").forceSetNumber(enabled ? 3: 1); } diff --git a/src/main/java/frc/robot/Manipulation.java b/src/main/java/frc/robot/Manipulation.java index eb46498..29f434e 100644 --- a/src/main/java/frc/robot/Manipulation.java +++ b/src/main/java/frc/robot/Manipulation.java @@ -59,7 +59,7 @@ public void setIntakeSpin(boolean spin) { * @param extend true if it should extend, false if not. */ public void setIntakeExtend(boolean extend) { - // intakePneumatics.set(extend ? Value.kForward : Value.kReverse); + intakePneumatics.set(extend ? Value.kForward : Value.kReverse); } /** diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d7a5681..9059e72 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -50,6 +50,7 @@ public class Robot extends TimedRobot { PowercellDetection detector; Shooter shooter = null; Drivetrain drive = null; + AutoAim aim; XboxController driver = null; Manipulation manipulation = null; XboxController operator = null; @@ -57,12 +58,18 @@ public class Robot extends TimedRobot { Compressor compressor = null; // Booleans for toggling different things... - boolean limelightToggle = false; + boolean limelightToggle = true; boolean photoswitchSensorToggle = false; boolean shooterToggle = true; boolean drivetrainToggle = true; + double targetAngle = 0; + + double speed = 0; + boolean aiming = false; + boolean autoAimToggle = true; + boolean manipulationToggle = true; - boolean navXToggle = false; + boolean navXToggle = true; boolean powercellDetectorToggle = false; boolean ptoEngaged = false; @@ -153,25 +160,33 @@ public void robotInit() { System.out.println("Shooter disabled. Skipping initialization..."); } + if (this.navXToggle) { + System.out.print("Initializing gyro system (NavX)..."); + gyro = new AHRS(SPI.Port.kMXP); + gyro.enableLogging(false); + + System.out.println("done"); + } else { + System.out.println("Gyro system (NavX) disabled. Skipping initialization..."); + } + if (this.drivetrainToggle) { System.out.print("Initializing drivetrain..."); Solenoid pto = new Solenoid(2, 2); DriveModule leftModule = new DriveModule(new TalonFX(5), new TalonFX(6), new TalonFX(7), pto); DriveModule rightModule = new DriveModule(new TalonFX(8), new TalonFX(9), new TalonFX(10), pto); - drive = new Drivetrain(leftModule, rightModule, detector); + drive = new Drivetrain(leftModule, rightModule, detector, gyro); System.out.println("done"); } else { System.out.println("Drivetrain disabled. Skipping initialization..."); } - if (this.navXToggle) { - System.out.print("Initializing gyro system (NavX)..."); - gyro = new AHRS(SPI.Port.kMXP); - gyro.enableLogging(false); - + if (this.autoAimToggle) { + System.out.print("Initializing automatic aiming system..."); + aim = new AutoAim(drive, limelight, shooter, gyro); System.out.println("done"); } else { - System.out.println("Gyro system (NavX) disabled. Skipping initialization..."); + System.out.println("Automatic aiming disabled. Skipping initialization..."); } System.out.print("Initializing driver interface..."); @@ -180,7 +195,7 @@ public void robotInit() { System.out.println("done"); System.out.print("Initializing compressor..."); - // compressor = new Compressor(2); + compressor = new Compressor(2); System.out.println("done"); } @@ -195,28 +210,33 @@ public void autonomousPeriodic() { @Override public void teleopInit() { // shooter.reHome(); + limelight.setLightEnabled(false); } @Override public void teleopPeriodic() { if (this.limelightToggle) { limelight.update(); - - if (driver.getXButtonPressed()) + if (driver.getXButtonPressed()) { limelight.setLightEnabled(true); - else if (driver.getYButtonPressed()) + } else if (driver.getYButtonPressed()) { limelight.setLightEnabled(false); + } } - if (this.powercellDetectorToggle) { - detector.update(); - } + // if (this.powercellDetectorToggle) { + // detector.update(); + // } if (this.shooterToggle) { - double speed = 0; double shooterAngleSpeed = 0; if (operator.getTriggerAxis(Hand.kRight) > 0.5) { + // if (operator.getYButton()) { + // speed -= 0.01; + // } else if (operator.getXButton()) { + // speed += 0.01; + // } speed = -1 * operator.getY(Hand.kRight); shooterAngleSpeed = operator.getY(Hand.kLeft); } @@ -229,7 +249,7 @@ else if (driver.getYButtonPressed()) shooter.reHome(); } - if (shooter.getState() == Shooter.State.Idle || shooter.getState() == Shooter.State.ManualControl) { + if ((shooter.getState() == Shooter.State.Idle || shooter.getState() == Shooter.State.ManualControl) && !aiming) { shooter.manualControl(speed, shooterAngleSpeed); } shooter.update(); @@ -280,13 +300,33 @@ else if (driver.getYButtonPressed()) // } // drive.tankDrive(leftInput, rightInput); - drive.arcadeDrive(turnInput, speedInput); + if (!aiming) { + drive.arcadeDrive(turnInput, speedInput); + } if (driver.getBButtonPressed()) { ptoEngaged = !ptoEngaged; drive.setPTO(ptoEngaged); } } + + if (this.autoAimToggle) { + if (aim.getState() == AutoAim.AutoAimState.IDLE) { + aiming = false; + } + + boolean driverYPressed = driver.getYButtonPressed(); + if (driverYPressed && !aiming) { + aiming = true; + aim.resetState(); + } else if (driverYPressed && aiming) { + aiming = false; + aim.stopState(); + } + + // limelight.setLightEnabled(aiming); + aim.run(); + } } @Override