From 01957ce6de48540017c8f21a0b8c129ae31772a6 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sun, 6 Mar 2022 13:02:17 -0700 Subject: [PATCH] Shoot Command Works now Untested on robot, but in sim --- src/main/java/frc4388/robot/Constants.java | 3 +- .../java/frc4388/robot/RobotContainer.java | 29 +++++----- src/main/java/frc4388/robot/RobotMap.java | 6 +- .../java/frc4388/robot/commands/Shoot.java | 57 +++++++++++++------ 4 files changed, 58 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 15517e1..d169b96 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -172,7 +172,8 @@ public final class Constants { public static final double ENCODER_TICKS_PER_REV = 2048; // Shoot Command Constants - public static final Gains SHOOT_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOT_DRIVE_GAINS = new Gains(5.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOT_TURRET_GAINS = new Gains(2.0, 0.0, 0.0, 0.0, 0, 1.0); /* Turret Constants */ // ID diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0b5cc75..52cf52e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -95,10 +95,10 @@ public class RobotContainer { private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); // private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter); // private final LED m_robotLED = new LED(m_robotMap.LEDController); - // private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); - // private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); - // private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); - // private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); + private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); + private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); + private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); + private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -196,7 +196,6 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kB.value) .whileHeld(new RunCommand(() -> m_robotSerializer.setSerializer(-0.25))) .whenReleased(new RunCommand(() -> m_robotSerializer.setSerializer(0.0))); - } // new JoystickButton(getDriverController(), XboxController.Button.kA.value) // .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); @@ -210,25 +209,25 @@ public class RobotContainer { /* Operator Buttons */ // X > Extend Intake - /*new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) .whenPressed(() -> m_robotIntake.runExtender(true)); // Y > Retract Intake new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(() -> m_robotIntake.runExtender(false));*/ + .whenPressed(() -> m_robotIntake.runExtender(false)); // Right Bumper > Storage In - /*new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - // Left Bumper > Storage Out (note: neccessary?) - new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) + // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) + // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + // // Left Bumper > Storage Out (note: neccessary?) + // new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) + // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) + // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); // A > Shoot with Odo new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); // B > Shoot with Lime new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));/* + .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index db64420..4f3ec54 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -163,7 +163,7 @@ public class RobotMap { // Shooter Config /* Boom Boom Subsystem */ - /*public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); + public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); // turret subsystem @@ -202,7 +202,7 @@ public class RobotMap { shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, - ShooterConstants.SHOOTER_TIMEOUT_MS);*/ + ShooterConstants.SHOOTER_TIMEOUT_MS); // /* Turret Subsytem */ // shooterFalconRight.configStatorCurrentLimit(new @@ -215,7 +215,7 @@ public class RobotMap { // hood subsystem // angleAdjusterMotor.restoreFactoryDefaults(); // angleAdjusterMotor.setIdleMode(IdleMode.kBrake); -// } + } diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java index 4899c71..65edc04 100644 --- a/src/main/java/frc4388/robot/commands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/Shoot.java @@ -40,16 +40,20 @@ public class Shoot extends CommandBase { // pid public double error; public double prevError; - public Gains shootGains = ShooterConstants.SHOOT_GAINS; + public Gains driveGains = ShooterConstants.SHOOT_DRIVE_GAINS; + public Gains turretGains = ShooterConstants.SHOOT_TURRET_GAINS; public double kP, kI, kD; public double proportional, integral, derivative; public double time; public double output; + public double normOutput; public double tolerance; public boolean isAimedInTolerance; + public int inverted; // testing - public DummySensor dummy = new DummySensor(0); + public DummySensor driveDummy; + public DummySensor turretDummy; /** * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball @@ -68,9 +72,9 @@ public class Shoot extends CommandBase { addRequirements(m_swerve, m_boomBoom, m_turret, m_hood); - kP = shootGains.kP; - kI = shootGains.kI; - kD = shootGains.kD; + kP = driveGains.kP; + kI = driveGains.kI; + kD = driveGains.kD; proportional = 0; integral = 0; @@ -80,6 +84,9 @@ public class Shoot extends CommandBase { tolerance = 5.0; isAimedInTolerance = false; + driveDummy = new DummySensor(180); + turretDummy = new DummySensor(180); + DummySensor.resetAll(); } @@ -87,17 +94,22 @@ public class Shoot extends CommandBase { * Updates error for custom PID. */ public void updateError() { - error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360; + m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get()); + // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees()); + error = (m_targetAngle - turretDummy.get() + 360) % 360; + // error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360; isAimedInTolerance = (Math.abs(error) <= tolerance); SmartDashboard.putBoolean("isAimed?", isAimedInTolerance); + System.out.println("Target Angle: " + m_targetAngle); + System.out.println("Error: " + error); } // Called when the command is initially scheduled. @Override public void initialize() { - m_odoX = m_swerve.getOdometry().getX(); - m_odoY = m_swerve.getOdometry().getY(); - m_distance = Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2)); + m_odoX = 0;//m_swerve.getOdometry().getX(); + m_odoY = -1;//m_swerve.getOdometry().getY(); + m_distance = Math.hypot(m_odoX, m_odoY);//Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2)); m_gyroAngle = m_swerve.getRegGyro().getDegrees(); @@ -105,11 +117,6 @@ public class Shoot extends CommandBase { m_targetVel = m_boomBoom.getVelocity(m_distance); m_targetHood = m_boomBoom.getHood(m_distance); - // target angle tests - m_gyroAngle = 0; - m_odoX = -1; - m_odoY = 1; - m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.; // deadzone processing @@ -131,6 +138,13 @@ public class Shoot extends CommandBase { * Run custom PID. */ public void runPID() { + if (error > 180){ + error = 360 - error; + inverted = -1; + } + else{ + inverted = 1; + } prevError = error; updateError(); @@ -138,20 +152,27 @@ public class Shoot extends CommandBase { integral = integral + error * time; derivative = (error - prevError) / time; output = kP * proportional + kI * integral + kD * derivative; + normOutput = output/360 * inverted; } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - + System.out.println("Normalized Output: " + normOutput); // custom pid runPID(); - // m_swerve.driveWithInput(0, 0, output, true); // i have no idea if this is how you rotate the + driveDummy.apply(normOutput); + System.out.println("Drive Dummy: " + driveDummy.get()); + m_swerve.driveWithInput(0, 0, normOutput, true); // i have no idea if this is how you rotate the // entire swerve drive or its the line below - m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true); + // m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true); m_hood.runAngleAdjustPID(m_targetHood); m_boomBoom.runDrumShooterVelocityPID(m_targetVel); + + turretDummy.apply(normOutput); + System.out.println("Turret Dummy: " + turretDummy.get()); + m_turret.m_boomBoomRotateMotor.set(normOutput); } // Called once the command ends or is interrupted. @@ -161,6 +182,6 @@ public class Shoot extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return isAimedInTolerance; } }