Vision improvements (#3)

* Start drive train code Co-authored-by: Julian Sprague <julspr@users.noreply.github.com> Co-authored-by: Dogloverblue <Dogloverblue@users.noreply.github.com> Co-authored-by: nguyenjerry2608@gmail.com <nguyenjerry2608@gmail.com> Co-authored-by: 222mi.ash@gmail.com <222mi.ash@gmail.com> Co-authored-by: atsuke-axolotl <atsuke-axolotl@users.noreply.github.com> Co-authored-by: Patricia-R <Patricia-R@users.noreply.github.com> * Fix April Tag size * make a class for simply moving the robot * improve vision * Add Jack's wrapper class Co-authored-by: DogLoverPink <DogLoverPink@users.noreply.github.com> Co-authored-by: BookSerpent <BookSerpent@users.noreply.github.com> Co-authored-by: Julian Sprague <julspr@users.noreply.github.com> Co-authored-by: 222mi.ash@gmail.com <222mi.ash@gmail.com> * Vision Improvments * Add mechanical wheel test Co-authored-by: Julian Sprague <julspr@users.noreply.github.com> Co-authored-by: nguyenjerry2608@gmail.com <nguyenjerry2608@gmail.com> Co-authored-by: 222mi.ash@gmail.com <222mi.ash@gmail.com> --------- Co-authored-by: Julian Sprague <julspr@users.noreply.github.com> Co-authored-by: Dogloverblue <Dogloverblue@users.noreply.github.com> Co-authored-by: nguyenjerry2608@gmail.com <nguyenjerry2608@gmail.com> Co-authored-by: 222mi.ash@gmail.com <222mi.ash@gmail.com> Co-authored-by: atsuke-axolotl <atsuke-axolotl@users.noreply.github.com> Co-authored-by: Patricia-R <Patricia-R@users.noreply.github.com> Co-authored-by: DogLoverPink <DogLoverPink@users.noreply.github.com> Co-authored-by: BookSerpent <BookSerpent@users.noreply.github.com>
WTMCRobotics · Feb 8, 2024 · 8925289 · 8925289
1 parent 9180bc5
commit 8925289
Show file tree

Hide file tree

Showing 18 changed files with 1,464 additions and 19 deletions.
diff --git a/.vscode/launch.json b/.vscode/launch.json
@@ -4,6 +4,13 @@
     // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
     "version": "0.2.0",
     "configurations": [
+        {
+            "type": "java",
+            "name": "Main",
+            "request": "launch",
+            "mainClass": "frc.robot.Main",
+            "projectName": "2024-Crescendo"
+        },
         {
             "type": "wpilib",
             "name": "WPILib Desktop Debug",

diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -28,5 +28,6 @@
     "java.test.defaultConfig": "WPIlibUnitTests",
     "editor.formatOnSave": true,
     "prettier.documentSelectors": ["**/*.java"],
-    "editor.defaultFormatter": "esbenp.prettier-vscode"
+    "editor.defaultFormatter": "esbenp.prettier-vscode",
+    "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -3,10 +3,10 @@
 public class Constants {
 
     /*DRIVE TRAIN MOTOR IDs*/
-    public static final int DRIVE_LEFT_PARENT_ID = 0;
-    public static final int DRIVE_LEFT_CHILD_ID = 1;
-    public static final int DRIVE_RIGHT_PARENT_ID = 2;
-    public static final int DRIVE_RIGHT_CHILD_ID = 3;
+    public static final int DRIVE_LEFT_PARENT_ID = 20;
+    public static final int DRIVE_LEFT_CHILD_ID = 22;
+    public static final int DRIVE_RIGHT_PARENT_ID = 21;
+    public static final int DRIVE_RIGHT_CHILD_ID = 23;
 
     /*CONTROLLER PORT IDs*/
     public static final int DRIVER_CONTROLLER_ID = 0;
@@ -17,4 +17,23 @@ public class Constants {
     public static final int CAMERA_BIT_CORRECTION_AMOUNT = 0;
     /**This is the amount of frames in a row that an april tag needs in frame to be to be considered confidently an april tag */
     public static final int APRIL_TAG_CONFIDENCE_FRAMES = 3;
+    public static final double APRIL_TAG_ROTATION_ZONE = 5;
+
+    /*PID GAINS *///0.00001, 100
+    public static final Gains ROTATION_GAINS = new Gains(1, 0.000, 0, 0, 0, 0.3);
+
+    /*PYSICAL ROBOT CONSTANTS */
+    public static final double WHEEL_CIRCUMFERENCE_INCHES = 0;
+
+    /* Jack helper class thjatshoudl probably have been retired */
+    /** Which PID slot to pull gains from */
+    public static final int SLOT_IDX = 0;
+    /** Which PID loop to pull gains from */
+    public static final int PID_LOOP_IDX = 0;
+    /** amount of time in ms to wait for confirmation */
+    public static final int TIMEOUT_MS = 30;
+    public static final double ENCODER_ROTATION = 4096.0;
+
+    /* ROTATION PID CONSTANTS */
+    public static final double ROTATION_ERROR_DEGREES = 5.;
 }
diff --git a/src/main/java/frc/robot/Gains.java b/src/main/java/frc/robot/Gains.java
@@ -0,0 +1,32 @@
+/**
+ * Class that organizes gains used when assigning values to slots
+ */
+package frc.robot;
+
+public class Gains {
+
+    /**
+     * Pelvic
+     */
+    public double P;
+    /**
+     * Inflammatory
+     */
+    public double I;
+    /**
+     * Disease
+     */
+    public double D;
+    public final double F;
+    public final int IZONE;
+    public double PEAK_OUTPUT;
+
+    public Gains(double p, double i, double d, double f, int izone, double peakOutput) {
+        P = p;
+        I = i;
+        D = d;
+        F = f;
+        IZONE = izone;
+        PEAK_OUTPUT = peakOutput;
+    }
+}
diff --git a/src/main/java/frc/robot/QuickActions.java b/src/main/java/frc/robot/QuickActions.java
@@ -0,0 +1,51 @@
+package frc.robot;
+
+import frc.robot.motor.MotorController;
+
+public class QuickActions {
+
+    static MotorController leftParent;
+    static MotorController leftChild;
+    static MotorController rightParent;
+    static MotorController rightChild;
+
+    public static void setDriveMotors(MotorController leftParentMotor, MotorController rightParentMotor) {
+        leftParent = leftParentMotor;
+        rightParent = rightParentMotor;
+    }
+
+    public static void setAll(double percent) {
+        leftParent.set(percent);
+        rightParent.set(percent);
+    }
+
+    public static void stopAll() {
+        leftParent.set(0.0);
+        rightParent.set(0.0);
+    }
+
+    public static void turn(TurnDirection direction, double percent) {
+        if (direction == TurnDirection.LEFT) {
+            leftParent.set(-percent);
+            rightParent.set(percent);
+        } else if (direction == TurnDirection.RIGHT) {
+            leftParent.set(percent);
+            rightParent.set(-percent);
+        }
+    }
+
+    public static void turnVelocity(TurnDirection direction, double percent) {
+        if (direction == TurnDirection.LEFT) {
+            leftParent.setVelocity(-percent);
+            rightParent.setVelocity(percent);
+        } else if (direction == TurnDirection.RIGHT) {
+            leftParent.setVelocity(percent);
+            rightParent.setVelocity(-percent);
+        }
+    }
+
+    public enum TurnDirection {
+        LEFT,
+        RIGHT,
+    }
+}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -4,25 +4,46 @@
 
 package frc.robot;
 
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Vision.AprilTagHighlighter;
+import frc.robot.motor.MotorController;
+import frc.robot.motor.MotorControllerFactory;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
- * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * each mode, as described in the TimedRo\bot documentation. If you change the name of this class or
  * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
 public class Robot extends TimedRobot {
 
     AprilTagHighlighter aprilTagHighlighter;
-    Spark driveLeftParent = new Spark(Constants.DRIVE_LEFT_PARENT_ID);
-    Spark driveLeftChild = new Spark(Constants.DRIVE_LEFT_CHILD_ID);
-    Spark driveRightParent = new Spark(Constants.DRIVE_RIGHT_PARENT_ID);
-    Spark driveRightChild = new Spark(Constants.DRIVE_RIGHT_CHILD_ID);
+    MotorController driveLeftParent = MotorControllerFactory.create(
+        Constants.DRIVE_LEFT_PARENT_ID,
+        MotorController.Type.Talon
+    );
+    MotorController driveLeftChild = MotorControllerFactory.create(
+        Constants.DRIVE_LEFT_CHILD_ID,
+        MotorController.Type.Talon
+    );
+    MotorController driveRightParent = MotorControllerFactory.create(
+        Constants.DRIVE_RIGHT_PARENT_ID,
+        MotorController.Type.Talon
+    );
+    MotorController driveRightChild = MotorControllerFactory.create(
+        Constants.DRIVE_RIGHT_CHILD_ID,
+        MotorController.Type.Talon
+    );
     XboxController driverController = new XboxController(Constants.DRIVER_CONTROLLER_ID);
+    static AHRS navX = new AHRS(SPI.Port.kMXP);
+
+    public static AHRS getGyroscope() {
+        return navX;
+    }
 
     /**
      * This function is run when the robot is first started up and should be used for any
@@ -32,7 +53,18 @@ public class Robot extends TimedRobot {
     public void robotInit() {
         aprilTagHighlighter = new AprilTagHighlighter();
 
+        driveLeftChild.setInverted(true);
+        driveLeftParent.setInverted(true);
+
+        driveLeftChild.follow(driveLeftParent);
+        driveRightChild.follow(driveRightParent);
+
+        QuickActions.setDriveMotors(driveLeftParent, driveRightParent);
+        getGyroscope().reset();
         System.out.println(Constants.APRIL_TAG_CONFIDENCE_FRAMES);
+        SmartDashboard.putNumber("rotationGainsP", 1);
+        SmartDashboard.putNumber("rotationGainsI", 0);
+        SmartDashboard.putNumber("rotationGainsD", 0);
     }
 
     /**
@@ -45,6 +77,10 @@ public void robotInit() {
     @Override
     public void robotPeriodic() {
         aprilTagHighlighter.doEveryFrame();
+        SmartDashboard.putNumber("Gyro Reading", getGyroscope().getAngle());
+        Constants.ROTATION_GAINS.P = SmartDashboard.getNumber("rotationGainsP", kDefaultPeriod);
+        Constants.ROTATION_GAINS.I = SmartDashboard.getNumber("rotationGainsI", kDefaultPeriod);
+        Constants.ROTATION_GAINS.D = SmartDashboard.getNumber("rotationGainsD", kDefaultPeriod);
     }
 
     /**
@@ -91,7 +127,9 @@ public void autonomousPeriodic() {}
 
     /** This function is called once when teleop is enabled. */
     @Override
-    public void teleopInit() {}
+    public void teleopInit() {
+        aprilTagHighlighter.sequenceInitiated = false;
+    }
 
     /** This function is called periodically during operator control. */
     @Override
@@ -102,8 +140,8 @@ public void teleopPeriodic() {
         driveLeftParent.set(leftY);
         driveRightParent.set(rightY);
 
-        driveLeftChild.set(leftY);
-        driveRightChild.set(rightY);
+        aprilTagHighlighter.doEveryTeleopFrame(driverController);
+
     }
 
     /** This function is called once when the robot is disabled. */
@@ -116,7 +154,35 @@ public void disabledPeriodic() {}
 
     /** This function is called once when test mode is enabled. */
     @Override
-    public void testInit() {}
+    public void testInit() {
+        Thread testThread = new Thread(() -> {
+            try {
+                driveLeftParent.set(-0.5);
+                System.out.println("Left is spinning backwards");
+                SmartDashboard.putString("Test Status", "Left is spinning backwards");
+
+                // wait for 5 seconds
+                Thread.sleep(5000);
+                driveLeftParent.set(0.0);
+
+                driveRightParent.set(-0.5);
+                System.out.println("Left is no longer spinning and Right is spinning backwards");
+                SmartDashboard.putString("Test Status", "Left is no longer spinning and Right is spinning backwards");
+
+                // wait for 5 seconds pt 2 electric boogaloo
+                Thread.sleep(5000);
+                driveRightParent.set(0.0);
+
+                System.out.println("Right is no longer spinning");
+                SmartDashboard.putString("Test Status", "Right is no longer spinning");
+            } catch (InterruptedException e) {
+                // If an error occurs, this code will be ran:
+                e.printStackTrace();
+            }
+        });
+
+        testThread.start();
+    }
 
     /** This function is called periodically during test mode. */
     @Override

diff --git a/src/main/java/frc/robot/RobotMotors.java b/src/main/java/frc/robot/RobotMotors.java
@@ -0,0 +1,11 @@
+package frc.robot;
+
+import frc.robot.motor.MotorController;
+
+public class RobotMotors {
+
+    public static void setupMotors(MotorController leftMotor, MotorController rightMotor) {
+        leftMotor.setInverted(true);
+        rightMotor.setInverted(false);
+    }
+}