Monterey Bay Regional (#27)

* Initial test with chained paths * Start on auton sequences This introduces some semi-sketchy callback logic to InternalSubsystem to implement a "WaitUntilShotFired" command. Perhaps we should think of a better way to do this. * changed turret turntable CIM/Talon to NEO/SparkMax * Turret encoder constants and PID tuning framework * Small progress on tuning * Odometry based turret tracking * Tuned turret controller and implemented drivetrain tracking * Merge Ethan's feedforward / relative angle code * Fix discrepancies in cherry picking * Flywheel configuring * Copy flywheel gear ratio from `develop`, rebase * Driver override for intake with 2 second buffer * Intake limit switch * Ring light toggling * Rudimentary turntable FF * Turntable limit switches, angle constraining * Turret lazy tracking * Turntable testing, runtime error fixes * Turret interpolation framework * climb testing code (without turret testing, comment in/out as needed) * Climb port shuffling * Fixed intake logic bug * PID constants after turret tuning * Hood constants * Reenable turret angle locking * Use GRTNetworkTableEntry to hopefully improve loop overrun * Use NetworkTable callbacks * Changed shoot command to not use a callback * Touched up turret PID, remove threaded pose estimation, reset pose on construct * Flywheel PID tuning setup * Tune flywheel PIDF * Add ball coordinates to constants * Tune min and max angles, theta FF * Shooter testing * Measure encoder + path following constants The measured `kP` seems scarily high, but unsure if this is a SysId issue or not (I had trouble getting data analysis to work properly). Will test hopefully soon. * Add interpolation table entries, fix logic * Merge `rtheta` into `shooter-testing`, closes #24 * Turret setInitialPose method * Working `rtheta` + interpolation * Initial position setting in RobotContainer * Clean things up, add more debug flags * Correct `setInitialPose` logic * Tweaks for easier testing + jetson thread sleeps + jetson debugging + L + ratio * Flywheel disabling when robot is moving * Read latest jetson line * Tuned intake deploy PID * Networktable callbacks, add periodic PID back * Stop entrance if 5 seconds have passed * More debug flags We should test the tolerance logic * Finish red sequences, `GRTAutonSequence` abstraction * Blue auton sequences * Auton sequence abstraction * Distance and theta offsets, controller bindings * Distance offset in interpolation logic * Preliminary testing routine * Merge with new offset methods * Camera server automatic capture, cleanup, constants * Merge auton code, more cleanup * Offset and intake gamepad tweaks * smoll debugs * Intake 'auto deploy' returns to state position when rollers are not powered, rather than default of 'RAISED' * Climb PID testing framework * Fix intake logic error, turret cleanup * add turret freezing and tweak driver commands to match doc * Some cleanup * Jetson disabling on shuffleboard, hopefully fix intake * Manual climb control on right mech joystick * Intake internals check shuffleboard button, auton ball localization change * Force shot when button pressed twice * Move climb to right mech joystick * Debug flag to skip auton if needed * Fix climb, troubleshoot shooter * more comp tuning * turret low hub mode * Shuffleboard auton sequences as int * drivetrain slowmode, shooting logic shifts, beginning of pleb auto * climb tweaks * more adjustments, post-comp push * low hub lock turret * Internals transition states * Decorate auton with timeouts * Interpolation clamping * Clamp distance offset between max-min and -(max-min) * Pleb auton path shot requested logic * Skip auto option, driver override flywheel logic * Auto 2 ball fixes * GRTNetworkTable abstraction * final comp tweaks * Even more shuffleboard abstraction, pilot with turret Co-authored-by: eggaskin <[email protected]> Co-authored-by: unknown <[email protected]> Co-authored-by: pinkbluesky <[email protected]>
grt192 · Mar 28, 2022 · 2f9cae8 · 2f9cae8
1 parent c8916d9
commit 2f9cae8
Show file tree

Hide file tree

Showing 29 changed files with 2,031 additions and 662 deletions.
diff --git a/.gitignore b/.gitignore
@@ -1,3 +1,7 @@
+# silly intellij/android studio things
+.idea/
+local.properties
+
 # This gitignore has been specially created by the WPILib team.
 # If you remove items from this file, intellisense might break.
 

diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -25,5 +25,6 @@
       }
     },
   ],
-  "java.test.defaultConfig": "WPIlibUnitTests"
+  "java.test.defaultConfig": "WPIlibUnitTests",
+  "wpilib.autoStartRioLog": true
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -4,7 +4,10 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.util.Color;
+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.util.Units;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide
@@ -31,6 +34,9 @@ public static final class TurretConstants {
         public static final int turntablePort = 17;
         public static final int hoodPort = 16;
         public static final int flywheelPort = 15;
+
+        public static final int lLimitSwitchPort = 4;
+        public static final int rLimitSwitchPort = 5;
     }
 
     public static final class InternalConstants {
@@ -46,14 +52,15 @@ public static final class InternalConstants {
     public static final class IntakeConstants {
         public static final int intakePort = 12;
         public static final int deploymentPort = 11;
+        public static final int limitSwitchPort = 3;
     }
 
     public static final class ClimbConstants {
-        public static final int sixMotorPort = 0;
-        public static final int sixBrakePort = 1;
+        public static final int sixMotorPort = 3;
+        public static final int sixBrakePort = 2;
 
-        public static final int tenMotorPort = 3;
-        public static final int tenBrakePort = 2;
+        public static final int tenMotorPort = 1;
+        public static final int tenBrakePort = 0;
 
         public static final int tenLeftSolenoidPort = 18;
         public static final int tenRightSolenoidPort = 18;
@@ -72,4 +79,34 @@ public static final class JetsonConstants {
     public static final class ShuffleboardConstants {
         public static final double UPDATE_TIME = 0.5; // seconds between each update
     }
+
+    /**
+     * Ball coordinates, based off of Kepler's work on onshape:
+     * https://pausd.onshape.com/documents/8003f7997b588f082d72c477/w/14e5661479750659f282841e/e/4af5ee78827743d1d23aca5f
+     * All coordinates assume the field coordinate system (positive x is right, positive y is up, theta ccw from
+     * x axis) with the origin at the hub.
+     */
+    public static final class BallCoordinates {
+        // Tarmac ball translations. These do not include angle because we may choose to approach these balls at
+        // different angles based on our starting position. Declared in clockwise order around the tarmac from 
+        // top right to top left ball.
+        public static final Translation2d 
+            RIGHT_TOP_BLUE = new Translation2d(Units.inchesToMeters(-33.763), Units.inchesToMeters(149.233)),
+            RIGHT_TOP_RED = new Translation2d(Units.inchesToMeters(25.907), Units.inchesToMeters(150.795)),
+            RIGHT_MID_RED = new Translation2d(Units.inchesToMeters(124.952), Units.inchesToMeters(88.302)),
+            RIGHT_MID_BLUE = new Translation2d(Units.inchesToMeters(149.230), Units.inchesToMeters(33.772)),
+            RIGHT_BOTTOM_RED = new Translation2d(Units.inchesToMeters(129.397), Units.inchesToMeters(-81.649)),
+            RIGHT_BOTTOM_BLUE = new Translation2d(Units.inchesToMeters(88.309), Units.inchesToMeters(-124.947)),
+            LEFT_BOTTOM_RED = new Translation2d(Units.inchesToMeters(33.763), Units.inchesToMeters(-149.233)),
+            LEFT_BOTTOM_BLUE = new Translation2d(Units.inchesToMeters(-25.907), Units.inchesToMeters(-150.795)),
+            LEFT_MID_BLUE = new Translation2d(Units.inchesToMeters(-124.952), Units.inchesToMeters(-88.302)),
+            LEFT_MID_RED = new Translation2d(Units.inchesToMeters(-149.230), Units.inchesToMeters(-33.772)),
+            LEFT_TOP_BLUE = new Translation2d(Units.inchesToMeters(-129.397), Units.inchesToMeters(81.649)),
+            LEFT_TOP_RED = new Translation2d(Units.inchesToMeters(-88.309), Units.inchesToMeters(124.947));
+
+        // Terminal ball translations. These should generally be approached at 45 and 225 degrees, respectively.
+        public static final Translation2d
+            TERMINAL_RED = new Translation2d(Units.inchesToMeters(284.210), Units.inchesToMeters(120.435)),
+            TERMINAL_BLUE = new Translation2d(Units.inchesToMeters(-284.210), Units.inchesToMeters(-120.435));
+    }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -4,10 +4,17 @@
 
 package frc.robot;
 
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj.Timer;
+
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+import frc.robot.subsystems.tank.TankSubsystem;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -19,16 +26,21 @@ public class Robot extends TimedRobot {
     private Command autonomousCommand;
 
     private RobotContainer robotContainer;
+    private PowerDistribution powerDistribution;
 
     /**
      * This function is run when the robot is first started up and should be used for any
      * initialization code.
      */
     @Override
     public void robotInit() {
-        // Instantiate our RobotContainer.    This will perform all our button bindings, and put our
+        // Instantiate our RobotContainer. This will perform all our button bindings, and put our
         // autonomous chooser on the dashboard.
         robotContainer = new RobotContainer();
+        powerDistribution = new PowerDistribution();
+
+        // Start camera server automatic capture and disable livewindow telemetry
+        CameraServer.startAutomaticCapture();
         LiveWindow.disableAllTelemetry();
     }
 
@@ -41,9 +53,9 @@ public void robotInit() {
      */
     @Override
     public void robotPeriodic() {
-        // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+        // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
         // commands, running already-scheduled commands, removing finished or interrupted commands,
-        // and running subsystem periodic() methods.  This must be called from the robot's periodic
+        // 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();
 
@@ -53,7 +65,10 @@ public void robotPeriodic() {
 
     /** This function is called once each time the robot enters Disabled mode. */
     @Override
-    public void disabledInit() {}
+    public void disabledInit() {
+        // Turn off the ring light when the robot disables
+        powerDistribution.setSwitchableChannel(false);
+    }
 
     @Override
     public void disabledPeriodic() {}
@@ -65,8 +80,8 @@ public void autonomousInit() {
         autonomousCommand = robotContainer.getAutonomousCommand();
         if (autonomousCommand != null) autonomousCommand.schedule();
 
-        // Set InternalSubsystem initial ball count
-        robotContainer.getInternalSubsystem().setAutonInitialBallCount();
+        // Turn on the ring light
+        powerDistribution.setSwitchableChannel(true);
     }
 
     /** This function is called periodically during autonomous. */
@@ -79,9 +94,10 @@ public void teleopInit() {
         // teleop starts running. If you want the autonomous to
         // continue until interrupted by another command, remove
         // this line or comment it out.
-        if (autonomousCommand != null) {
-            autonomousCommand.cancel();
-        }
+        if (autonomousCommand != null) autonomousCommand.cancel();
+
+        // Turn on the ring light
+        powerDistribution.setSwitchableChannel(true);
     }
 
     /** This function is called periodically during operator control. */
@@ -90,8 +106,61 @@ public void teleopPeriodic() {}
 
     @Override
     public void testInit() {
+        TankSubsystem tankSubsystem = robotContainer.getTankSubsystem();
+        TurretSubsystem turretSubsystem = robotContainer.getTurretSubsystem();
+        IntakeSubsystem intakeSubsystem = robotContainer.getIntakeSubsystem();
+
         // Cancels all running commands at the start of test mode.
         CommandScheduler.getInstance().cancelAll();
+
+        // Turn on the ring light
+        powerDistribution.setSwitchableChannel(true);
+
+        // Run DT turning left and right alternating
+        tankSubsystem.setCarDrivePowers(0, 1); // Rotate DT right
+        Timer.delay(2);
+        tankSubsystem.setCarDrivePowers(0, -1); // Rotate DT left
+        Timer.delay(2);
+        tankSubsystem.setCarDrivePowers(0, 0);
+
+        // Sweep intake from -1 to 1 power
+        intakeSubsystem.setDriverOverride(true);
+        for (double pow = -1; pow <= 1; pow += 0.1) {
+            intakeSubsystem.setIntakePower(pow);
+            Timer.delay(0.1);
+        }
+        intakeSubsystem.setDriverOverride(false);
+
+        // Alternate intake deploy
+        for (int i = 0; i < 2; i++) {
+            intakeSubsystem.setPosition(IntakeSubsystem.IntakePosition.DEPLOYED);
+            Timer.delay(4);
+            intakeSubsystem.setPosition(IntakeSubsystem.IntakePosition.RAISED);
+            Timer.delay(4);
+        }
+
+        // Sweep turntable (sequence: left, center, right, center)
+        double turretTimeDelta = 1.5;  // Seconds to rotate turret 180
+        turretSubsystem.changeTurntableOffset(Math.toRadians(-180));
+        Timer.delay(turretTimeDelta);
+        turretSubsystem.changeTurntableOffset(-Math.toRadians(-180));
+        Timer.delay(turretTimeDelta);
+        turretSubsystem.changeTurntableOffset(Math.toRadians(180));
+        Timer.delay(turretTimeDelta);
+        turretSubsystem.changeTurntableOffset(-Math.toRadians(180));
+
+        // Run flywheel at a low speed 
+        turretSubsystem.setFlywheelVel(1000);
+        Timer.delay(5);
+        turretSubsystem.setFlywheelVel(0);
+
+        // Sweep hood up and down twice
+        for (int i = 0; i < 2; i++) {
+            turretSubsystem.setHoodPos(TurretSubsystem.HOOD_MAX_POS);
+            Timer.delay(1.5);
+            turretSubsystem.setHoodPos(TurretSubsystem.HOOD_MIN_POS);
+            Timer.delay(1.5);
+        }
     }
 
     /** This function is called periodically during test mode. */