diff --git a/build.gradle b/build.gradle index e58767a..55600bc 100644 --- a/build.gradle +++ b/build.gradle @@ -1,4 +1,4 @@ -import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO +import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" @@ -28,7 +28,7 @@ deploy { frcJava(getArtifactTypeClass('FRCJavaArtifact')) { // Enable visualvm - jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2" + //jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2" } // Static files artifact diff --git a/src/main/deploy/Distances.csv b/src/main/deploy/Distances.csv new file mode 100644 index 0000000..e69de29 diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv new file mode 100644 index 0000000..99413bc --- /dev/null +++ b/src/main/deploy/Robot Data - Distances.csv @@ -0,0 +1,3 @@ +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) +0 ,16 ,12000 +999 ,28.8 ,12000 \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Every Ball.path b/src/main/deploy/pathplanner/Every Ball.path new file mode 100644 index 0000000..d1e4271 --- /dev/null +++ b/src/main/deploy/pathplanner/Every Ball.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":6.888836249581911,"y":5.215172677538834},"prevControl":null,"nextControl":{"x":6.537459367052471,"y":6.518965846916928},"holonomicAngle":106.3895403340348,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.8254588419278255,"y":7.8320057763675734},"prevControl":{"x":5.95491348285958,"y":6.759381608647329},"nextControl":{"x":5.95491348285958,"y":6.759381608647329},"holonomicAngle":-136.27303002005684,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.085718036603518,"y":6.18608248452099},"prevControl":{"x":5.222728278411091,"y":6.456690985532331},"nextControl":{"x":4.768933589337623,"y":5.5604025113498725},"holonomicAngle":-109.50244850666218,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.77803093326875,"y":3.4953054389840252},"prevControl":{"x":5.623399779842569,"y":3.7042882657946703},"nextControl":{"x":2.9363292766883204,"y":3.040020127851594},"holonomicAngle":-30.256437163529373,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.7486154042034336,"y":2.667447141251462},"prevControl":{"x":1.8144326730676037,"y":2.686839729398941},"nextControl":{"x":0.7297128839172015,"y":2.3672347915242677},"holonomicAngle":-75.06858282186249,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2129730034385093,"y":1.1845800804778424},"prevControl":{"x":1.2001001569914929,"y":1.2299758967179297},"nextControl":{"x":1.530279149856384,"y":0.06560677694911808},"holonomicAngle":-39.472459848343846,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.160119378376086,"y":1.9305622828301685},"prevControl":{"x":5.131309514738882,"y":1.9153315905888442},"nextControl":{"x":6.588402375563037,"y":2.6856418291137163},"holonomicAngle":36.02737338510347,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.60730489584927,"y":0.37491647060743843},"prevControl":{"x":7.329902093852659,"y":0.5875919521381767},"nextControl":{"x":7.884707697845882,"y":0.16224098907670015},"holonomicAngle":-41.00908690157027,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":8.617110072204374,"y":0.9389517943373179},"prevControl":{"x":8.535233976824232,"y":0.12019084053588047},"nextControl":{"x":8.698986167584517,"y":1.7577127481387553},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":9.144756020209744,"y":0.31123506308954896},"prevControl":{"x":9.199340083796505,"y":0.7206155399902687},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path new file mode 100644 index 0000000..9c09583 --- /dev/null +++ b/src/main/deploy/pathplanner/New Path.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":1.0,"y":3.0},"prevControl":null,"nextControl":{"x":2.3149413387434365,"y":2.75478019310469},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.143744996350752,"y":3.8428026223141054},"prevControl":{"x":1.0069994823533945,"y":5.5442845062905315},"nextControl":{"x":7.478686694138279,"y":2.0338121072780666},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.870800551727686,"y":4.710905624342894},"prevControl":{"x":6.870800551727686,"y":4.710905624342894},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Two Ball High to Low.path b/src/main/deploy/pathplanner/Two Ball High to Low.path new file mode 100644 index 0000000..43221e3 --- /dev/null +++ b/src/main/deploy/pathplanner/Two Ball High to Low.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":6.620680207650139,"y":5.35387407853639},"prevControl":null,"nextControl":{"x":6.047381083523801,"y":5.557302800000574},"holonomicAngle":169.99202019855858,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.020990716136325,"y":6.167588964393128},"prevControl":{"x":5.409354638931585,"y":5.5018222396012515},"nextControl":{"x":5.409354638931585,"y":5.5018222396012515},"holonomicAngle":129.4995885297982,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.141198597001524,"y":1.9325728539114686},"prevControl":{"x":5.085718036602201,"y":3.2825998236283294},"nextControl":null,"holonomicAngle":-77.66091272167375,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Two Ball Low.path b/src/main/deploy/pathplanner/Two Ball Low.path new file mode 100644 index 0000000..f43098c --- /dev/null +++ b/src/main/deploy/pathplanner/Two Ball Low.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":8.109408578365308,"y":2.117508055242545},"prevControl":null,"nextControl":{"x":8.192629418964295,"y":1.470234850583776},"holonomicAngle":-92.202598161766,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.637823814971061,"y":0.36987040266386867},"prevControl":{"x":8.775175303157184,"y":0.48083152346251423},"nextControl":{"x":6.123786635522426,"y":0.22215945832741724},"holonomicAngle":178.6360724683971,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.141198597001524,"y":1.9048325737118068},"prevControl":{"x":5.61278336039577,"y":1.729144132447284},"nextControl":null,"holonomicAngle":143.8418145601917,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":null,"maxAcceleration":5.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Two Ball Straight to Player Station.path b/src/main/deploy/pathplanner/Two Ball Straight to Player Station.path new file mode 100644 index 0000000..b4b5c9c --- /dev/null +++ b/src/main/deploy/pathplanner/Two Ball Straight to Player Station.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":6.777875128781554,"y":2.4688849377715907},"prevControl":null,"nextControl":{"x":5.797718561726847,"y":2.043533974710114},"holonomicAngle":-156.37062226934333,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2298190888492497,"y":1.174338528454054},"prevControl":{"x":1.904832573707682,"y":1.0171436073226392},"nextControl":null,"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 03e07ca..6aa7365 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -4,12 +4,15 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; + import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; + import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; @@ -52,11 +55,12 @@ public final class Constants { public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; public static final int GYRO_ID = 14; - // ofsets are in degrees - public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0; - public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0; - public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0; - public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0; + // offsets are in degrees + // NATHAN if you truncate or round or simplify these i will cry + public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45-3.30;//181.7578125;//180.0;//315.0 +45;//180.0; + public static final double RIGHT_FRONT_ENCODER_OFFSET = 360.-59.0625+0.18;//360.-59.0625;//315.0;//224.296875 + 90;//300.0; + public static final double LEFT_BACK_ENCODER_OFFSET = 360.-128.222;//308.408203125;//225.0;//45.87890625;//360.0 - 128.0; + public static final double RIGHT_BACK_ENCODER_OFFSET = 360.+2.15-3.637;//180-2.021484375;//0.0;//134.384765625 + 45; // swerve PID constants public static final int SWERVE_SLOT_IDX = 0; @@ -67,10 +71,10 @@ public final class Constants { // swerve auto constants public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0); public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0); - public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI)); + public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController (15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI)); public static final boolean PATH_RECORD_VELOCITY = true; - public static final double PATH_MAX_VELOCITY = 5.5; - public static final double PATH_MAX_ACCELERATION = 50; + public static final double PATH_MAX_VELOCITY = 5.0; + public static final double PATH_MAX_ACCELERATION = 5.0; public static final double MIN_WAYPOINT_ANGLE = 20; public static final double MIN_WAYPOINT_DISTANCE = 0.1; public static final double MIN_WAYPOINT_VELOCITY = 0.1; @@ -81,10 +85,11 @@ public final class Constants { public static final int REMOTE_0 = 0; // conversions - // gear ratio: 5.12 rev motor = 1 rev wheel + // gear ratio: 5.14 rev motor = 1 rev wheel // wheel diameter: official = 4 in, measured = 3.8 in /* Ratio Calculation */ - public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; + public static final double MOTOR_REV_PER_STEER_REV = 12.8; + public static final double MOTOR_REV_PER_WHEEL_REV = 5.142857; public static final double WHEEL_DIAMETER_INCHES = 4.0; public static final double TICKS_PER_MOTOR_REV = 2048; public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; @@ -119,4 +124,62 @@ public final class Constants { public static final double LEFT_AXIS_DEADBAND = 0.1; public static final double RIGHT_AXIS_DEADBAND = 0.6; } + + public static final class ShooterConstants { + /* PID Constants Shooter */ + public static final int CLOSED_LOOP_TIME_MS = 1; + + public static final int SHOOTER_TIMEOUT_MS = 32; + public static final int SHOOTER_SLOT_IDX = 0; + public static final int SHOOTER_PID_LOOP_IDX = 1; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); + public static final int SHOOTER_FALCON_LEFT_CAN_ID = 23; + public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 24; + public static final int SHOOTER_ROTATE_ID = 31; // TODO: find + public static final double TURRET_SPEED_MULTIPLIER = 0.1d; + public static final int DEGREES_PER_ROT = 0; + 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; + + + + /* Turret Constants */ + //ID + public static final int TURRET_MOTOR_CAN_ID = 30; + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); + public static final double SHOOTER_TURRET_MIN = -1.0; + public static final float TURRET_FORWARD_LIMIT = 0; // TODO: find + public static final float TURRET_REVERSE_LIMIT = 0; // TODO: find + + // deadzones + public static final double HARD_DEADZONE_LEFT = 0.0; + public static final double HARD_DEADZONE_RIGHT = 340.0; + + public static final double DIG_DEADZONE_LEFT = 40.0; + public static final double DIG_DEADZONE_RIGHT = 60.0; + + public static final int SHOOTER_FALCON_BALLER_ID = 0; // TODO: find + public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"// + + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0,0,0,0,0,0); // TODO: tune values + + /* Hood Constants */ + public static final int SHOOTER_ANGLE_ADJUST_ID = 32; + public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find + public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find + public static final float HOOD_FORWARD_LIMIT = 0; //TODO: find + public static final float HOOD_REVERSE_LIMIT = 0; //TODO: find + + + } + public static final class VisionConstants { + public static final double TURN_P_VALUE = 0.8; + public static final double X_ANGLE_ERROR = 0.5; + public static final double GRAV = 385.83; + public static final double TARGET_HEIGHT = 67.5; + public static final double FOV = 29.8; //Field of view limelight + public static final double LIME_ANGLE = 24.7; + } } diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java index 8d25e15..9fbaf8f 100644 --- a/src/main/java/frc4388/robot/Main.java +++ b/src/main/java/frc4388/robot/Main.java @@ -35,5 +35,4 @@ public final class Main { } } - -// hi ryan \ No newline at end of file +// hi ryan -aarav \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 4ab5f9a..c4edf74 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -105,6 +105,8 @@ public class Robot extends TimedRobot { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + + // print odometry data to smart dashboard for debugging (if causing timeout errors, you can comment it) SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX()); SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY()); SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees()); @@ -132,9 +134,6 @@ public class Robot extends TimedRobot { @Override public void disabledPeriodic() { - // SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX()); - // SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY()); - // SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees()); } /** @@ -162,6 +161,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { LOGGER.fine("teleopInit()"); + m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove @@ -178,8 +178,6 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - // m_robotContainer.getDriverController().updateInput(); - // m_robotContainer.getOperatorController().updateInput(); } @Override diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4f44b21..952372c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -53,8 +53,13 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.commands.AimToCenter; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.Vision; import frc4388.utility.LEDPatterns; import frc4388.utility.ListeningSendableChooser; import frc4388.utility.PathPlannerUtil; @@ -74,11 +79,13 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( - m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); 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(); + private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); + private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom); /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -99,7 +106,14 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); /* Default Commands */ - // drives the swerve drive with a two-axis input from the driver controller + + // continually sends updates to the Blinkin LED controller to keep the lights on + // m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); + + //Turret default command + + m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive)); + m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), @@ -123,9 +137,9 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ - new JoystickButton(getDriverController(), XboxController.Button.kY.value) - // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON) - .whenPressed(m_robotSwerveDrive.m_gyro::reset); + // "XboxController.Button.kBack" was undefined yet, 7 works just fine + new JoystickButton(getDriverController(), 7) + .whenPressed(m_robotSwerveDrive::resetGyro); new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON) @@ -141,9 +155,16 @@ public class RobotContainer { /* Operator Buttons */ // activates "Lit Mode" new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + // activates "BoomBoom" + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1)) + .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)); + // activates hood + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whenPressed(() -> m_robotHood.runHood(0.5d)) + .whenReleased(() -> m_robotHood.runHood(0.d)); } /** @@ -177,10 +198,18 @@ public class RobotContainer { return m_driverXbox; } + /** + * Get odometry. + * @return Odometry + */ public Pose2d getOdometry() { return m_robotSwerveDrive.getOdometry(); } + /** + * Set odometry to given pose. + * @param pose Pose to set odometry to. + */ public void resetOdometry(Pose2d pose) { m_robotSwerveDrive.resetOdometry(pose); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index faefdd5..4f1478f 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -5,13 +5,20 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.RemoteSensorSource; +import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; @@ -24,16 +31,18 @@ public class RobotMap { public RobotMap() { configureLEDMotorControllers(); configureSwerveMotorControllers(); + configureShooterMotorControllers(); } /* LED Subsystem */ public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); void configureLEDMotorControllers() { - + } - + /* Swerve Subsystem */ + public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); @@ -46,6 +55,7 @@ public class RobotMap { public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); + public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID); public SwerveModule leftFront; @@ -91,14 +101,15 @@ public class RobotMap { rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake); - leftFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast - rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake); - rightFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast - leftBackSteerMotor.setNeutralMode(NeutralMode.Brake); - leftBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast - rightBackSteerMotor.setNeutralMode(NeutralMode.Brake); - rightBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast + NeutralMode mode = NeutralMode.Coast; + leftFrontSteerMotor.setNeutralMode(mode); + leftFrontWheelMotor.setNeutralMode(mode);//Coast + rightFrontSteerMotor.setNeutralMode(mode); + rightFrontWheelMotor.setNeutralMode(mode);//Coast + leftBackSteerMotor.setNeutralMode(mode); + leftBackWheelMotor.setNeutralMode(mode);//Coast + rightBackSteerMotor.setNeutralMode(mode); + rightBackWheelMotor.setNeutralMode(mode);//Coast leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); @@ -106,10 +117,56 @@ public class RobotMap { rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); // config cancoder as remote encoder for swerve steer motors - //leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + } + + // Shooter Config + /* Boom Boom Subsystem */ + 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); + + public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); + + // Create motor CANSparkMax + void configureShooterMotorControllers() { + + // LEFT FALCON + shooterFalconLeft.configFactoryDefault(); + shooterFalconLeft.setNeutralMode(NeutralMode.Coast); + shooterFalconLeft.setInverted(true); + shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + + // RIGHT FALCON + shooterFalconRight.setInverted(false); + shooterFalconRight.setNeutralMode(NeutralMode.Coast); + shooterFalconRight.configFactoryDefault(); + shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + // m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) + shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + + /* Turret Subsytem */ + shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers out of our ass anymore + shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull numbers out of our ass anymore + } } diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java new file mode 100644 index 0000000..45f0998 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -0,0 +1,78 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; + +public class AimToCenter extends CommandBase { + /** Creates a new AimWithOdometry. */ + Turret m_turret; + SwerveDrive m_drive; + + // use odometry to find x and y later + double x; + double y; + double m_targetAngle; + + // public static Gains m_aimGains; + + public AimToCenter(Turret turret, SwerveDrive drive) { + // Use addRequirements() here to declare subsystem dependencies. + m_turret = turret; + m_drive = drive; + addRequirements(m_turret, m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + x = 0; + y = 0; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); + m_turret.runshooterRotatePID(m_targetAngle); + } + + public static double angleToCenter(double x, double y, double gyro) { + double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) + return angle; + } + + /** + * Checks if in hardware deadzone (due to mechanical limitations). + * @param angle Angle to check. + * @return True if in hardware deadzone. + */ + public static boolean isHardwareDeadzone(double angle) { + return ((ShooterConstants.HARD_DEADZONE_LEFT > angle) || (angle > ShooterConstants.HARD_DEADZONE_RIGHT)); + } + + /** + * Checks if in digital deadzone (due to climber). + * @param angle Angle to check. + * @return True if in digital deadzone. + */ + public static boolean isDigitalDeadzone(double angle) { + return ((ShooterConstants.DIG_DEADZONE_LEFT < angle) && (angle < ShooterConstants.DIG_DEADZONE_RIGHT)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java new file mode 100644 index 0000000..28a5f3e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Shoot.java @@ -0,0 +1,157 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; + +public class Shoot extends CommandBase { + + // subsystems + public SwerveDrive m_swerve; + public BoomBoom m_boomBoom; + public Turret m_turret; + public Hood m_hood; + + // given + public double m_gyroAngle; + public double m_odoX; + public double m_odoY; + public double m_distance; + + // targets + public double m_targetVel; + public double m_targetHood; + public double m_targetAngle; + public double m_driveTargetAngle; + + // pid + public double error; + public double prevError; + public double kP, kI, kD; + public double proportional, integral, derivative; + public double time; + public double output; + public double tolerance = 5.0; + + // // dummy motor + // public WPI_TalonFX dummy = new WPI_TalonFX(69 - 420); + // public TalonFXConfiguration dummyConfiguration = new TalonFXConfiguration(); + + /** Creates a new Shoot. */ + public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) { + // Use addRequirements() here to declare subsystem dependencies. + m_swerve = sDrive; + m_boomBoom = sShooter; + m_turret = sTurret; + m_hood = sHood; + + addRequirements(m_swerve, m_boomBoom, m_turret, m_hood); + + kP = 0.1; + kI = 0.0; + kD = 0.0; + + proportional = 0; + integral = 0; + derivative = 0; + time = 0.02; + } + + /** + * Updates error for custom PID. + */ + public void updateError() { + error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_odoX = 0; //TODO: get this value using odometry + m_odoY = 0; //TODO: get this value using odometry + m_distance = Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2)); + + m_gyroAngle = m_swerve.getRegGyro().getDegrees(); + + // get targets (shooter tables) + m_targetVel = m_boomBoom.getVelocity(m_distance); + m_targetHood = m_boomBoom.getHood(m_distance); + m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.; + m_driveTargetAngle = m_targetAngle + m_turret.getBoomBoomAngleDegrees(); + + // // normal (i think) PID stuff + // dummyConfiguration.primaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.IntegratedSensor.toFeedbackDevice(); + // dummyConfiguration.remoteFilter0.remoteSensorDeviceID = dummy.getDeviceID(); + // dummyConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor; + + // dummyConfiguration.slot0.kP = 0.1; + // dummyConfiguration.slot0.kI = 0; + // dummyConfiguration.slot0.kD = 0; + // dummyConfiguration.slot0.kF = 0; + + // // weird PID stuff + // dummyConfiguration.auxiliaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.SoftwareEmulatedSensor.toFeedbackDevice(); + // dummyConfiguration.remoteFilter1.remoteSensorDeviceID = ShooterConstants.TURRET_MOTOR_CAN_ID; + // dummyConfiguration.remoteFilter1.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor; + // // dummyConfiguration.auxiliaryPID.selectedFeedbackCoefficient = 0; + + // dummyConfiguration.slot1.kP = 0.1; + // dummyConfiguration.slot1.kI = 0; + // dummyConfiguration.slot1.kD = 0; + // dummyConfiguration.slot1.kF = 0; + + // dummy.configAllSettings(dummyConfiguration); + + // initial error + updateError(); + prevError = error; + } + + /** + * Run custom PID. + */ + public void runPID() { + prevError = error; + updateError(); + + proportional = error; + integral = integral + error * time; + derivative = (error - prevError) / time; + output = kP * proportional + kI * integral + kD * derivative; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // dummy.selectProfileSlot(0, 0); + // dummy.selectProfileSlot(1, 1); + // dummy.set(TalonFXControlMode.Position, m_driveTargetAngle, DemandType.AuxPID, m_targetAngle); + // m_swerve.driveWithInput(0, 0, m_driveTargetAngle, true); + // m_swerve.driveWithInput(0, 0, Math.cos(m_driveTargetAngle), Math.sin(m_driveTargetAngle), true); // only works for new DWI in swerve branch + + // custom pid + runPID(); + m_swerve.driveWithInput(0, 0, output, true); + + m_hood.runAngleAdjustPID(m_targetHood); + m_boomBoom.runDrumShooterVelocityPID(m_targetVel); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + updateError(); + return Math.abs(error) <= tolerance; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java new file mode 100644 index 0000000..64e7027 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -0,0 +1,142 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import java.io.File; +import java.io.IOException; +import java.util.Comparator; +import java.util.Map; +import java.util.Optional; +import java.util.function.Function; +import java.util.regex.Pattern; +import java.util.stream.IntStream; + +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.CSV; +import frc4388.utility.Gains; + +public class BoomBoom extends SubsystemBase { + public WPI_TalonFX m_shooterFalconLeft; + public WPI_TalonFX m_shooterFalconRight; + public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; + public static BoomBoom m_boomBoom; + + double velP; + double input; + + public boolean m_isDrumReady = false; + public double m_fireVel; + + public Hood m_hoodSubsystem; + public Turret m_turretSubsystem; + + // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values + // later + + public static class ShooterTableEntry { + public Double distance, hoodExt, drumVelocity; + } + + private ShooterTableEntry[] m_shooterTable; + + /* + * Creates new BoomBoom subsystem, has drum shooter and angle adjuster + */ + /** Creates a new BoomBoom. */ + public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { + m_shooterFalconLeft = shooterFalconLeft; + m_shooterFalconRight = shooterFalconRight; + + try { + CSV csv = new CSV<>(ShooterTableEntry::new) { + private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)"); + + @Override + protected String headerSanitizer(final String header) { + return super.headerSanitizer(parentheses.matcher(header).replaceAll("")); + } + + }; + m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath()); + new Thread(() -> System.out.println(CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start(); + } catch (final IOException e) { + e.printStackTrace(); + // throw new RuntimeException(e); + } + } + + public Double getVelocity(final Double distance) { + return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); + } + + public Double getHood(final Double distance) { + return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); + } + + private static Number linearInterpolate(final E[] table, final Number lookupValue, final Function lookupGetter, final Function targetGetter) { + final Map.Entry closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1])); + final E closestRecord = closestEntry.getValue(); + final int closestRecordIndex = closestEntry.getKey(); + final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))]; + return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord)); + } + + private static Optional> lookup(final E[] table, final Number value, final Function valueGetter, final boolean exactMatch) { + final Optional> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue()))); + return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty(); + } + + private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) { + final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue()); + return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue(); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + + } + + public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) { + m_hoodSubsystem = subsystem0; + m_turretSubsystem = subsystem1; + } + + /** + * Runs the Drum motor at a given speed + * @param speed percent output form -1.0 to 1.0 + */ + public void runDrumShooter(double speed) { + m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); + + } + + public void setShooterGains() { + m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); + m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); + } + + public void runDrumShooterVelocityPID(double targetVel) { + m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init + m_shooterFalconRight.follow(m_shooterFalconLeft); + // New BoomBoom controller stuff + // Controls a motor with the output of the BangBang controller + // Controls a motor with the output of the BangBang conroller and a feedforward + // Shrinks the feedforward slightly to avoid over speeding the shooter + + // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * + // feedforward.calculate(targetVel)); + // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java new file mode 100644 index 0000000..b3a2f30 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -0,0 +1,96 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.Gains; + +public class Hood extends SubsystemBase { + public BoomBoom m_shooterSubsystem; + + public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public SparkMaxLimitSwitch m_hoodUpLimitSwitch; + public SparkMaxLimitSwitch m_hoodDownLimitSwitch; + public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; + public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); + + public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); + + + //public boolean m_isHoodReady = false; + +public double m_fireAngle; + + + /** Creates a new Hood. */ + public Hood() { + m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + + // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_hoodUpLimitSwitch.enableLimitSwitch(true); + m_hoodDownLimitSwitch.enableLimitSwitch(true); + + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT); + setHoodSoftLimits(true); + } + + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Set status of hood motor soft limits. + * @param set Boolean to set soft limits to. + */ + public void setHoodSoftLimits(boolean set) { + m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); + } + + public void runAngleAdjustPID(double targetAngle) + { + //Set PID Coefficients + m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP); + m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI); + m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD); + m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone); + m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF); + m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput); + + m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); + } + + + public void runHood(double input) { + // m_angleAdjusterMotor.set(input); + } + + public void resetGyroAngleAdj(){ + // m_angleEncoder.setPosition(0); + } + + public double getAnglePosition(){ + return 0.0;//m_angleEncoder.getPosition(); + } + + public double getAnglePositionDegrees(){ + return 0.0;//((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index a8b36f6..c5b530b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -54,37 +54,19 @@ public class SwerveDrive extends SubsystemBase { public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; - public Rotation2d rotTarget = new Rotation2d();; + public Rotation2d rotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); private final Field2d m_field = new Field2d(); public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, WPI_PigeonIMU gyro) { - // m_leftFrontSteerMotor = leftFrontSteerMotor; - // m_leftFrontWheelMotor = leftFrontWheelMotor; - // m_rightFrontSteerMotor = rightFrontSteerMotor; - // m_rightFrontWheelMotor = rightFrontWheelMotor; - // m_leftBackSteerMotor = leftBackSteerMotor; - // m_leftBackWheelMotor = leftBackWheelMotor; - // m_rightBackSteerMotor = rightBackSteerMotor; - // m_rightBackWheelMotor = rightBackWheelMotor; - // m_leftFrontEncoder = leftFrontEncoder; - // m_rightFrontEncoder = rightFrontEncoder; - // m_leftBackEncoder = leftBackEncoder; - // m_rightBackEncoder = rightBackEncoder; + m_leftFront = leftFront; m_leftBack = leftBack; m_rightFront = rightFront; m_rightBack = rightBack; m_gyro = gyro; - // modules = new SwerveModule[] { - // new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left - // new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right - // new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left - // new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right - // }; - modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack}; m_poseEstimator = @@ -154,21 +136,26 @@ public class SwerveDrive extends SubsystemBase { SwerveModuleState state = desiredStates[i]; module.setDesiredState(state, false); } + // modules[0].setDesiredState(desiredStates[0], false); } @Override public void periodic() { - updateOdometry(); - SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus)); - SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); - SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle()); - SmartDashboard.putNumber("Pigeon Rotation 2D", m_gyro.getRotation2d().getDegrees()); - SmartDashboard.putStringArray("Fusion Status", new String[] {"Is Fusing: "+fstatus.bIsFusing, "Is Valid: "+fstatus.bIsValid, "Heading: "+fstatus.heading}); - // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); + updateOdometry(); + // SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus)); + + SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); + + // SmartDashboard.putNumber("Front Left", modules[0].driveMotor.getSelectedSensorPosition()); + // SmartDashboard.putNumber("Front Right", modules[1].driveMotor.getSelectedSensorPosition()); + // SmartDashboard.putNumber("Back Left", modules[2].driveMotor.getSelectedSensorPosition()); + // SmartDashboard.putNumber("Back Right", modules[3].driveMotor.getSelectedSensorPosition()); + // SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle()); + // SmartDashboard.putNumber("Pigeon Rotation 2D", m_gyro.getRotation2d().getDegrees()); + // SmartDashboard.putStringArray("Fusion Status", new String[] {"Is Fusing: "+fstatus.bIsFusing, "Is Valid: "+fstatus.bIsValid, "Heading: "+fstatus.heading}); + + // m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); super.periodic(); } @@ -208,27 +195,29 @@ public class SwerveDrive extends SubsystemBase { } /** - * Resets the odometry of the robot to (x=0, y=0, theta=0). + * Gets the current gyro using regression formula. + * @return Rotation2d object holding current gyro in radians + */ + public Rotation2d getRegGyro() { + double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527; + return new Rotation2d(regCur * Math.PI / 180); + } + + /** + * Resets the odometry of the robot to the given pose. */ public void resetOdometry(Pose2d pose) { - // m_odometry.resetPosition(pose, m_gyro.getRotation2d()); m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d()); } - /** Updates the field relative position of the robot. */ - + /** Updates the field relative position of the robot. + */ public void updateOdometry() { - m_poseEstimator.update( m_gyro.getRotation2d(), + m_poseEstimator.update( getRegGyro(), modules[0].getState(), modules[1].getState(), modules[2].getState(), modules[3].getState()); - - // m_odometry.update( m_gyro.getRotation2d(), - // modules[0].getState(), - // modules[1].getState(), - // modules[2].getState(), - // modules[3].getState()); // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on // a real robot, this must be calculated based either on latency or timestamps. @@ -242,6 +231,9 @@ public class SwerveDrive extends SubsystemBase { rotTarget = new Rotation2d(0); } + /** + * Stop all four swerve modules. + */ public void stopModules() { modules[0].stop(); modules[1].stop(); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index bc3d6f2..44afb70 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -7,6 +7,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; @@ -15,19 +16,27 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; public class SwerveModule extends SubsystemBase { - private WPI_TalonFX driveMotor; - private WPI_TalonFX angleMotor; + public WPI_TalonFX angleMotor; + public WPI_TalonFX driveMotor; private CANCoder canCoder; public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; private static double kEncoderTicksPerRotation = 4096; private SwerveModuleState state; private double canCoderFeedbackCoefficient; + + public long m_currentTime; + public long m_lastTime; + public double m_deltaTime; + + public double m_currentPos; + public double m_lastPos; /** Creates a new SwerveModule. */ public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) { @@ -48,21 +57,38 @@ public class SwerveModule extends SubsystemBase { angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; angleMotor.configAllSettings(angleTalonFXConfiguration); - TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration(); - driveTalonFXConfiguration.slot0.kP = 0.15; - driveTalonFXConfiguration.slot0.kI = 0.0; - driveTalonFXConfiguration.slot0.kD = 0.5; - driveMotor.configAllSettings(driveTalonFXConfiguration); + // TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration(); + // driveTalonFXConfiguration.slot0.kP = 0.05; + // driveTalonFXConfiguration.slot0.kI = 0.0; + // driveTalonFXConfiguration.slot0.kD = 0.0; + // driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.IntegratedSensor; + driveMotor.configFactoryDefault(); + driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); + driveMotor.configNominalOutputForward(0, 30); + driveMotor.configNominalOutputReverse(0, 30); + driveMotor.configPeakOutputForward(1, 30); + driveMotor.configPeakOutputReverse(-1, 30); + driveMotor.configAllowableClosedloopError(0, 0, 30); + driveMotor.config_kP(0, 0.5, 30); + driveMotor.config_kI(0, 0, 30); + driveMotor.config_kD(0, 0, 30); + // maybe try a feedforward value? + + // driveMotor.configAllSettings(driveTalonFXConfiguration); CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); canCoderConfiguration.magnetOffsetDegrees = offset; canCoder.configAllSettings(canCoderConfiguration); - } + m_currentTime = System.currentTimeMillis(); + m_lastTime = System.currentTimeMillis(); + + m_lastPos = driveMotor.getSelectedSensorPosition(); + } private Rotation2d getAngle() { // Note: This assumes the CANCoders are setup with the default feedback coefficient - // and the sesnor value reports degrees. + // and the sensor value reports degrees. return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); } @@ -72,8 +98,7 @@ public class SwerveModule extends SubsystemBase { */ public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) { Rotation2d currentRotation = getAngle(); - - //SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); + // SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); state = SwerveModuleState.optimize(desiredState, currentRotation); // Find the difference between our current rotational position + our new rotational position @@ -84,29 +109,62 @@ public class SwerveModule extends SubsystemBase { // Convert the CANCoder from it's position reading back to ticks double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient; double desiredTicks = currentTicks + deltaTicks; + if (!ignoreAngle){ angleMotor.set(TalonFXControlMode.Position, desiredTicks); } + double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond); + double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC; + // double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69; + + // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10); + driveMotor.set(normFtPerSec);// - angleMotor.get()); + // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1 - double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - // driveMotor.set(TalonFXControlMode.Velocity, /*angleMotor.get() + */(Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10); - driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC); + // m_currentTime = System.currentTimeMillis(); + // m_deltaTime = (double) (m_currentTime - m_lastTime); + // m_deltaTime = m_deltaTime / 10.0; + + // m_currentPos = driveMotor.getSelectedSensorPosition(); + + // double m_desiredCorrectionVel = 3.2 * angleMotor.getSelectedSensorVelocity(); + // double m_desiredCorrectionPos = (m_deltaTime * m_desiredCorrectionVel) % 2048; + // double m_lastPos = (driveMotor.getSelectedSensorPosition() % 2048) - (m_deltaTime * driveMotor.getSelectedSensorVelocity()); + // double m_actualDesiredPos = m_deltaTime * ((Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10); + + // System.out.println("Current Pos: " + driveMotor.getSelectedSensorPosition()); + // System.out.println("Desired Correction Pos: " + m_desiredCorrectionPos); + // System.out.println("Last Pos: " + m_lastPos); + + // driveMotor.set(TalonFXControlMode.Position, 1500/*m_desiredCorrectionPos*/); + + // m_lastTime = m_currentTime; + // m_lastPos = m_currentPos; } /** - * Returns the current state of the module. + * Get current module state. * - * @return The current state of the module. + * @return The current state of the module in m/s. */ public SwerveModuleState getState() { // return state; return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle()); } + /** + * Stop the drive and steer motors of current module. + */ public void stop() { driveMotor.set(0); angleMotor.set(0); } + @Override + public void periodic(){ + Rotation2d currentRotation = getAngle(); + SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); + SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(), ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java new file mode 100644 index 0000000..3a00f02 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -0,0 +1,115 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; + +import java.util.concurrent.TimeoutException; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.SparkMaxPIDController; + +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.commands.Shoot; +import frc4388.utility.Gains; + +public class Turret extends SubsystemBase { + + /** Creates a new Turret. */ + public BoomBoom m_boomBoomSubsystem; + public SwerveDrive m_sDriveSubsystem; + + public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, + // MotorType.kBrushless); + public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; + SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit; + public Gyro m_turretGyro; + + public double m_targetDistance = 0; + + public boolean m_isAimReady = false; + + SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController(); + public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); + + // Variables + public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument + + m_boomBoomRotateMotor = boomBoomRotateMotor; + m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); + m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); + m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); + + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit.enableLimitSwitch(true); + m_boomBoomLeftLimit.enableLimitSwitch(true); + SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); + SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); + + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_FORWARD_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_REVERSE_LIMIT); + setTurretSoftLimits(true); + + m_boomBoomRotateMotor.setInverted(false); + + m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); + m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); + m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); + m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); + m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); + m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Set status of turret motor soft limits. + * @param set Boolean to set soft limits to. + */ + public void setTurretSoftLimits(boolean set) { + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); + } + + public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) { + m_boomBoomSubsystem = subsystem0; + m_sDriveSubsystem = subsystem1; + } + + public void runTurretWithInput(double input) { + m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); + } + + public void runshooterRotatePID(double targetAngle) { + targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT; + m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition); + } + + public void resetGyroShooterRotate() { + m_boomBoomRotateEncoder.setPosition(0); + } + + public double getboomBoomRotatePosition() { + return m_boomBoomRotateEncoder.getPosition(); + } + + public double getBoomBoomAngleDegrees() { + return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360 + / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; + } + +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java new file mode 100644 index 0000000..748bcd5 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -0,0 +1,131 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.networktables.NetworkTableEntry; + +import frc4388.robot.Constants.VisionConstants; + +public class Vision extends SubsystemBase { +//setup + Turret m_turret; + BoomBoom m_boomBoom; + Hood m_hood; + +NetworkTableEntry xEntry; +//Aiming +double turnAmount = 0; +double xAngle = 0; +double yAngle = 0; +double target = 0; +public double distance; +public double realDistance; +public static double fireVel; +public static double fireAngle; + +public double m_hoodTrim; +public double m_turretTrim; +public double m_fireAngle; + + +public Vision(Turret aimSubsystem, BoomBoom boomBoom) { + m_turret = aimSubsystem; + m_boomBoom = boomBoom; + m_hood = m_boomBoom.m_hoodSubsystem; + //addRequirements(m_turret); + limeOff(); + changePipeline(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(3); +} + +public void track(){ + target = getV(); + xAngle = getX(); + yAngle = getY(); + + //find distance + distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); + realDistance = (1.09 * distance) - 12.8; + + // if (target == 1.0) { //checks if target is in view + // //aims left and right + // turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE); + // if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { + // turnAmount = 0; + // } + // else if (turnAmount > 0 && turnAmount < 0.1){ + // turnAmount = 0.1; + // } + // else if (turnAmount < 0 && turnAmount > -0.1){ + // turnAmount = -0.1; + // } + // } + + SmartDashboard.putNumber("Distance to Target", realDistance); + + + // //start CSV + + // fireVel = m_boomBoom.m_shooterTable.getVelocity(realDistance); + // fireAngle = m_boomBoom.m_shooterTable.getHood(realDistance); + // //fire angle unknown so far + // //end of CSV + + // m_boomBoom.m_fireVel = fireVel; + // m_hood.m_fireAngle = fireAngle; + // m_turret.m_targetDistance = distance; + + // checkFinished(); +} + +public void checkFinished(){ + if (xAngle < 0.5 && xAngle > -0.5 && target == 1){ + m_turret.m_isAimReady = true; + } + else{ + m_turret.m_isAimReady = false; + } +} + +public void limeOff(){ + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); +} + + public void limeOn(){ + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); +} + + public void changePipeline(int pipelineId) + { + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId); + } + + public double getV() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0); + } + + public double getX() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); + } + + public double getY() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); + } + @Override + public void periodic(){ + //called once per scheduler run + } +} + + diff --git a/src/main/java/frc4388/utility/CSV.java b/src/main/java/frc4388/utility/CSV.java new file mode 100644 index 0000000..45ac768 --- /dev/null +++ b/src/main/java/frc4388/utility/CSV.java @@ -0,0 +1,233 @@ +package frc4388.utility; + +import java.awt.Color; +import java.io.BufferedReader; +import java.io.IOException; +import java.lang.invoke.MethodHandleProxies; +import java.lang.invoke.MethodHandles; +import java.lang.invoke.MethodType; +import java.lang.reflect.Array; +import java.lang.reflect.Field; +import java.lang.reflect.Modifier; +import java.nio.file.Files; +import java.nio.file.Path; +import java.text.MessageFormat; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Objects; +import java.util.function.BiConsumer; +import java.util.function.Function; +import java.util.function.IntFunction; +import java.util.function.Predicate; +import java.util.function.Supplier; +import java.util.regex.Pattern; +import java.util.stream.Collectors; +import java.util.stream.IntStream; +import java.util.stream.Stream; + +public class CSV { + private static final Pattern SANITIZER = Pattern.compile("[^$\\w,]"); + + private final Supplier generator; + private final IntFunction arrayGenerator; + private final Map> setters; + + /** + * A binary string operator to be applied to the entire header of the CSV. + */ + protected String headerSanitizer(final String header) { + return SANITIZER.matcher(header).replaceAll(""); + } + + /** + * A binary string operator to be applied to each name in the header of the CSV. + */ + protected String nameProcessor(final String name) { + return Character.toLowerCase(name.charAt(0)) + name.substring(1); + } + + /** + * Creates a new {@code CSV} instance and prepares for populating the fields of objects created by + * the given generator. Private fields and fields of primitive types are not supported. + * @param generator a parameterless supplier which produces a new object with any number of fields + * corresponding to header names from a CSV file. The first character of the names + * from the header in the CSV file will be made lowercase and invalid characters + * will be removed to match Java naming conventions. + * @see #read(Path) + */ + @SuppressWarnings("unchecked") + public CSV(final Supplier generator) { + final Class clazz = generator.get().getClass(); + final Map, Function> fieldParsers = new HashMap<>(); + this.arrayGenerator = size -> (R[]) Array.newInstance(clazz, size); + this.generator = generator; + this.setters = new HashMap<>(); + for (final Field field : clazz.getFields()) { + final Function parser = Modifier.isStatic(field.getModifiers()) ? null : fieldParsers.computeIfAbsent(field.getType(), CSV::getTypeParser); + if (parser != null) + this.setters.put(field.getName(), (final R obj, final String rawValue) -> { + try { + field.set(obj, rawValue.isEmpty() ? null : parser.apply(rawValue)); + } catch (final IllegalAccessException e) { + throw new RuntimeException(e); + } + }); + } + } + + /** + * Reads and parses the contents of the given CSV file, and returns an array filled with populated + * objects created with the previously given generator. Cells are parsed using their corresponding + * field's {@code valueOf(String)} function. + * @param path the path to a CSV file + * @return the parsed data from the CSV file + * @throws IOException if an I/O error occurs opening the file + */ + @SuppressWarnings("unchecked") + public R[] read(final Path path) throws IOException { + try (final BufferedReader reader = Files.newBufferedReader(path)) { + final BiConsumer[] fieldSetters = Stream.of(headerSanitizer(reader.readLine()).split(",")).map(this::nameProcessor).map(setters::get).toArray(BiConsumer[]::new); + final Stream lines = reader.lines(); + return lines.filter(Predicate.not(String::isBlank)).map(line -> deserializeRecordString(line, fieldSetters, generator.get())).toArray(this.arrayGenerator); + } + } + + @SuppressWarnings("unchecked") + private static Function getTypeParser(final Class type) { + try { + return type.isAssignableFrom(String.class) ? Function.identity() : MethodHandleProxies.asInterfaceInstance(Function.class, MethodHandles.publicLookup().findStatic(type, "valueOf", MethodType.methodType(type, String.class))); + } catch (final NoSuchMethodException | IllegalAccessException e) { + return null; + } + } + + private static R deserializeRecordString(final String recordString, final BiConsumer[] fieldParseSetters, final R object) { + final int recordStringLength = recordString.length(); + int fieldBeginIndex = 0, tryFieldEndFromIndex = 0, i = 0; + while (tryFieldEndFromIndex < recordStringLength && i < fieldParseSetters.length) { + final int tryFieldEndIndex = recordString.indexOf(',', tryFieldEndFromIndex); + String field = recordString.substring(fieldBeginIndex, tryFieldEndIndex == -1 ? recordStringLength : tryFieldEndIndex).strip(); + if (!field.isEmpty() && (tryFieldEndFromIndex != fieldBeginIndex || field.charAt(0) == '"')) { + final int fieldLength = field.length(); + if (countTrailing(field, '"') % 2 == 0) { + tryFieldEndFromIndex = tryFieldEndIndex + 1; + continue; + } else + field = field.substring(1, fieldLength - 1).replace("\"\"", "\""); + } + final BiConsumer setter = fieldParseSetters[i]; + if (setter != null) + setter.accept(object, field); + tryFieldEndFromIndex = fieldBeginIndex = tryFieldEndIndex + 1; + i++; + } + return object; + } + + private static int countTrailing(final String str, final char c) { + final int l = str.length(); + int count = 0; + while (str.charAt(l - count - 1) == c && count < l) + count++; + return count; + } + + public static final class ReflectionTable { + public static String create(final T[] objects, boolean colored) { + final Field[] fields = Stream.of(objects).flatMap(object -> Stream.of(object.getClass().getFields())).distinct().toArray(Field[]::new); + final List> rows = new ArrayList<>(); + rows.add(Stream.of(fields).map(ReflectionTable::new).collect(Collectors.toList())); + rows.addAll(Stream.of(objects).map(obj -> Stream.of(fields).map(field -> new ReflectionTable(obj, field)).collect(Collectors.toList())).collect(Collectors.toList())); + final int[] columnWidths = rows.stream().map(row -> row.stream().map(cell -> cell.string).mapToInt(String::length).toArray()).reduce(new int[fields.length], (result, row) -> IntStream.range(0, row.length).map(i -> Math.max(result[i], row[i])).toArray()); + if (colored) + IntStream.range(0, fields.length).forEach(i -> { + final var columnSummaryStatistics = rows.stream().skip(1).mapToDouble(row -> row.get(i).getValue().doubleValue()).summaryStatistics(); + rows.stream().skip(1).forEach(row -> row.get(i).colorByValue(columnSummaryStatistics.getMin(), columnSummaryStatistics.getMax())); + }); + MessageFormat formatFormat = new MessageFormat(colored ? "{2} %{0}{1}s {3}" : " %{0}{1}s "); + return rows.stream().map(row -> IntStream.range(0, row.size()).mapToObj(i -> String.format(formatFormat.format(new Object[] { row.get(i).padRight ? "-" : "", columnWidths[i], row.get(i).escape, RESET_STYLE }), row.get(i).string)).collect(Collectors.joining("|"))).collect(Collectors.joining(LF)); + } + + private static final Color GRADIENT_MIN = new Color(0x00, 0x99, 0x00); + private static final Color GRADIENT_MAX = new Color(0x66, 0xFF, 0x66); + private static final String CONTROL = "\033"; + private static final String CSI = "["; + private static final String LF = "\n"; + private static final String RESET = "0"; + private static final String BOLD = "1"; + private static final String ITALIC = "3"; + private static final String UNDERLINE = "4"; + private static final String BACKGROUND_RED = "41"; + private static final String FOREGROUND = "38"; + private static final String BACKGROUND = "48"; + private static final String TRUECOLOR = "2"; + private static final String SEPARATOR = ";"; + private static final String SGR = "m"; + private static final String HEADER_STYLE = CONTROL + CSI + BOLD + SEPARATOR + UNDERLINE + SGR; + private static final String NULL_STYLE = CONTROL + CSI + ITALIC + SGR; + private static final String ERROR_STYLE = CONTROL + CSI + ITALIC + SGR + CONTROL + CSI + BACKGROUND_RED + SGR; + private static final String RESET_STYLE = CONTROL + CSI + RESET + SGR; + private Object value; + private String string; + private boolean padRight; + private String escape; + + private ReflectionTable(final Object obj, final Field field) { + try { + value = field.get(obj); + string = Objects.toString(value); + padRight = !Number.class.isAssignableFrom(field.getType()); + escape = Objects.isNull(value) ? NULL_STYLE : ""; + } catch (final IllegalAccessException | IllegalArgumentException e) { + value = null; + string = e.getClass().getSimpleName(); + padRight = false; + escape = ERROR_STYLE; + } + } + + private ReflectionTable(final Field field) { + value = null; + string = field.getName(); + padRight = true; + escape = HEADER_STYLE; + } + + private Number getValue() { + return padRight ? Objects.hashCode(string) : Objects.requireNonNullElse((Number) value, 0); + } + + private void colorByValue(final Number min, final Number max) { + if (Objects.nonNull(value)) { + final double range = max.doubleValue() - min.doubleValue(); + final double normal = range == 0 ? 0 : (getValue().doubleValue() - min.doubleValue()) / range; + final Color color = new Color(range(normal, GRADIENT_MIN.getRed(), GRADIENT_MAX.getRed()), range(normal, GRADIENT_MIN.getGreen(), GRADIENT_MAX.getGreen()), range(normal, GRADIENT_MIN.getBlue(), GRADIENT_MAX.getBlue())); + escape += (contrastRatio(color, Color.BLACK) > contrastRatio(Color.WHITE, color) ? colorTo24BitSGR(Color.BLACK, false) : colorTo24BitSGR(Color.WHITE, false)) + colorTo24BitSGR(color, true); + } + } + + private static String colorTo24BitSGR(final Color color, final boolean background) { + return CONTROL + CSI + (background ? BACKGROUND : FOREGROUND) + SEPARATOR + TRUECOLOR + SEPARATOR + color.getRed() + SEPARATOR + color.getGreen() + SEPARATOR + color.getBlue() + SGR; + } + + private static int range(final double normal, final int min, final int max) { + return (int) (normal * (max - min) + min); + } + + /* https://www.w3.org/TR/WCAG20/#contrast-ratiodef */ + private static float contrastRatio(final Color lighter, final Color darker) { + return (relativeLuminance(lighter) + 0.05f) / (relativeLuminance(darker) + 0.05f); + } + + /* https://www.w3.org/TR/2008/REC-WCAG20-20081211/#relativeluminancedef */ + private static float relativeLuminance(final Color color) { + final float[] components = color.getRGBComponents(null); + final float r = components[0] <= 0.03928f ? components[0] / 12.92f : (float) Math.pow((components[0] + 0.055f) / 1.055f, 2.4f); + final float g = components[1] <= 0.03928f ? components[1] / 12.92f : (float) Math.pow((components[1] + 0.055f) / 1.055f, 2.4f); + final float b = components[2] <= 0.03928f ? components[2] / 12.92f : (float) Math.pow((components[2] + 0.055f) / 1.055f, 2.4f); + return 0.2126f * r + 0.7152f * g + 0.0722f * b; + } + } +} diff --git a/src/test/java/frc4388/commands/AimToCenterTest.java b/src/test/java/frc4388/commands/AimToCenterTest.java new file mode 100644 index 0000000..72a84f4 --- /dev/null +++ b/src/test/java/frc4388/commands/AimToCenterTest.java @@ -0,0 +1,113 @@ +package frc4388.commands; + +import org.junit.Test; + +import frc4388.robot.commands.AimToCenter; +import org.junit.Assert; + +public class AimToCenterTest { + + private static final double DELTA = 1e-15; + + @Test + public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() { + boolean output; + + //20 deg + output = AimToCenter.isHardwareDeadzone(20.); + Assert.assertFalse(output); + + //-10 deg + output = AimToCenter.isHardwareDeadzone(-10.); + Assert.assertTrue(output); + + //-1 deg + output = AimToCenter.isHardwareDeadzone(-1.); + Assert.assertTrue(output); + + //341 deg + output = AimToCenter.isHardwareDeadzone(341.); + Assert.assertTrue(output); + + //340 deg + output = AimToCenter.isHardwareDeadzone(340.); + Assert.assertFalse(output); + + //0 deg + output = AimToCenter.isHardwareDeadzone(0.); + Assert.assertFalse(output); + + //200 deg + output = AimToCenter.isHardwareDeadzone(200.); + Assert.assertFalse(output); + + //2000000 deg + output = AimToCenter.isHardwareDeadzone(2000000.); + Assert.assertTrue(output); + + //NaN deg + output = AimToCenter.isHardwareDeadzone(Double.NaN); + Assert.assertFalse(output); + } + + @Test + public void givenOdometry_whenCalculateAngleToCenter_thenReturnAngleToCenter() { + double actual; + double expected; + + //(5,5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(5., 5., 0.); + expected = 180. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(-5,5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(-5.0, 5., 0.); + expected = 180. + 90. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(-5,-5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(-5.0, -5., 0.); + expected = 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(5,-5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(5., -5., 0.); + expected = 90. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(0,0) Gyro = 0 deg + actual = AimToCenter.angleToCenter(0., 0., 0.); + Assert.assertNotNull(actual); + + //(5,5) Gyro = 180 deg + actual = AimToCenter.angleToCenter(5., 5., 180.); + expected = 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(100,100) Gyro = 90 deg + actual = AimToCenter.angleToCenter(100., 100., 90.); + expected = 90. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(null,5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(Double.NaN, 5., 0.); + expected = Double.NaN; + Assert.assertEquals(expected, actual, DELTA); + + //(null,null) Gyro = 0 deg + actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, 0.); + expected = Double.NaN; + Assert.assertEquals(expected, actual, DELTA); + + //(null,null) Gyro = null deg + actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, Double.NaN); + expected = Double.NaN; + Assert.assertEquals(expected, actual, DELTA); + + //(5,5) Gyro = -20 deg + actual = AimToCenter.angleToCenter(5., -5., -45.); + expected = 180.; + Assert.assertEquals(expected, actual, DELTA); + + } +} diff --git a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java index 69cb243..a0f6b97 100644 --- a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java +++ b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java @@ -6,6 +6,7 @@ package frc4388.robot.subsystems; import static org.junit.Assert.assertEquals; // import static org.mockito.Mockito.mock; +import static org.mockito.Mockito.mock; import org.junit.Test; @@ -17,6 +18,9 @@ import frc4388.utility.LEDPatterns; * Add your docs here. */ public class LEDSubsystemTest { + // Arrange + Spark ledController = mock(Spark.class); + LED led = new LED(ledController); @Test public void testConstructor() { // Arrange diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..fb19ccf --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,73 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2022.1.1", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2022.1.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2022.1.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ] +}