From abc2e8b05fb4326dac47f6f889defcdd71ebf914 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Mon, 14 Mar 2022 16:56:32 -0600 Subject: [PATCH] track target distance regression WORKS --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/Robot.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 17 +++++++++++------ .../frc4388/robot/commands/TrackTarget.java | 4 +++- .../java/frc4388/robot/subsystems/Turret.java | 4 ++-- 5 files changed, 18 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d54fce3..c57bd9d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -190,7 +190,7 @@ public final class Constants { public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; public static final double TURRET_SPEED_MULTIPLIER = 0.6; - public static final double DEGREES_PER_ROT = 180.0/105.45445251464844; + public static final double TURRET_DEGREES_PER_ROT = 180.0/105.45445251464844; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; public static final double ENCODER_TICKS_PER_REV = 2048; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index ae7067e..b5c7845 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -121,7 +121,7 @@ public class Robot extends TimedRobot { }); desmosServer.start(); - DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0}); + // DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0}); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0e0c200..961b817 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -59,6 +59,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.OIConstants; +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.AimToCenter; @@ -291,11 +292,11 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new InstantCommand(() -> m_robotExtender.m_extenderMotor.getEncoder().setPosition(0))); - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); @@ -317,10 +318,14 @@ public class RobotContainer { // A > Shoot with Odo /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ + + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whenPressed(new RunCommand(() -> m_robotTurret.runshooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); //B > Shoot with Lime - // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) + .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))); /* Button Box Buttons */ diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index c1e4a14..2cd8924 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -112,12 +112,14 @@ public class TrackTarget extends CommandBase { } public final double distanceRegression(double distance) { - return m * distance + b; + return (1.09517561985 * distance + 20.1846165624); } public void updateRegressionDesmos() { m = DesmosServer.readDouble("m"); b = DesmosServer.readDouble("b"); + + DesmosServer.putArray("MB", m, b); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 00e141c..cf10e17 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -72,7 +72,7 @@ public class Turret extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); - SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.DEGREES_PER_ROT); + SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); } /** @@ -94,7 +94,7 @@ public class Turret extends SubsystemBase { } public void runshooterRotatePID(double targetAngle) { - targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT; + targetAngle = targetAngle / ShooterConstants.TURRET_DEGREES_PER_ROT; m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition); }