Leo swerve

.wpilib/wpilib_preferences.json

@@ -2,5 +2,5 @@
     "enableCppIntellisense": false,
     "currentLanguage": "java",
     "projectYear": "2024",
-    "teamNumber": 8503
+    "teamNumber": 4253
 }

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.3.1"
+    id "edu.wpi.first.GradleRIO" version "2024.3.2"
 }
 
 java {
@@ -69,15 +69,15 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
-    implementation 'com.github.shueja:Monologue:v1.0.0-beta5'
+    // implementation 'com.github.shueja:Monologue:v1.0.0-beta5'
 
     def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
     annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
 
 }
 
 repositories {
-    maven { url 'https://jitpack.io' }
+    // maven { url 'https://jitpack.io' }
     maven {
         url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit")
         credentials {

src/main/java/raidone/lib/util/ColorConverter.java

@@ -0,0 +1,49 @@
+package raidone.lib.util;
+
+public class ColorConverter {
+
+    public static int[] hueToRGB(float hue) {
+                // Convert hue from 0-255 range to 0-360 range
+                float hueInDegrees = (hue / 255.0f) * 360.0f;
+
+                float C = 1.0f; // Full saturation and brightness (chroma)
+                float X = C * (1 - Math.abs((hueInDegrees / 60.0f) % 2 - 1));
+                float m = 0.0f; // Lightness correction factor (since we're assuming full brightness)
+
+                float r = 0, g = 0, b = 0;
+
+                if (0 <= hueInDegrees && hueInDegrees < 60) {
+                    r = C;
+                    g = X;
+                    b = 0;
+                } else if (60 <= hueInDegrees && hueInDegrees < 120) {
+                    r = X;
+                    g = C;
+                    b = 0;
+                } else if (120 <= hueInDegrees && hueInDegrees < 180) {
+                    r = 0;
+                    g = C;
+                    b = X;
+                } else if (180 <= hueInDegrees && hueInDegrees < 240) {
+                    r = 0;
+                    g = X;
+                    b = C;
+                } else if (240 <= hueInDegrees && hueInDegrees < 300) {
+                    r = X;
+                    g = 0;
+                    b = C;
+                } else if (300 <= hueInDegrees && hueInDegrees < 360) {
+                    r = C;
+                    g = 0;
+                    b = X;
+                }
+
+                // Convert the RGB values to the range of 0 to 255
+                int red = Math.round((r + m) * 255);
+                int green = Math.round((g + m) * 255);
+                int blue = Math.round((b + m) * 255);
+
+                return new int[]{red, green, blue};
+    }
+
+}

src/main/java/raidone/robot/Constants.java

@@ -1,17 +1,23 @@
 package raidone.robot;
 
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
+import com.ctre.phoenix6.signals.ForwardLimitSourceValue;
+import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
+import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.ReverseLimitSourceValue;
+import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
 
-import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.math.util.Units;
 
 public final class Constants {
 
+    public static final String CANIVORE_NAME = "seCANdary";
+
     public static final class Swerve {
 
         public static final int PIGEON_ID = 0;
@@ -115,89 +121,87 @@ public static final class Swerve {
     public static final class Arm {
         public static final int ARM_MOTOR_ID = 9;
         public static final int ARM_FOLLOW_ID = 10;
-
-        public static final State SCORINGPOS = new State(-33, 0);
-        public static final double SOFTLIMIT = SCORINGPOS.position - 2;
-        public static final State INTAKEPOS = new State(0.0, 0);
-
-        public static final State CONSTRAINTPOS = new State(-13, 0);
-
-        public static final double kP = 0.17;
-        public static final double kI = 0.0;
-        public static final double kD = 0.01;
-        public static final double kIz = 0.0;
-        public static final double kFF = 0.0;
-
-        public static final double kS = 0.25;
-        public static final double kG = 0.35;// 0.47;
-        public static final double kV = 12.24;// 0.6;
-        public static final double kA = 0.01;
-
-        public static final ArmFeedforward FEED_FORWARD = new ArmFeedforward(kS, kG, kV, kA);
-
-        public static final double MAX_OUTPUT = 1.0;
-        public static final double MIN_OUTPUT = -1.0;
-        public static final double MAX_VEL_RPS = 24.5 / 2.0;
-        public static final double MAX_ACCEL_RPSS = MAX_VEL_RPS / 2.0;
-        public static final double ALLOWED_ERROR = 0.0;
-        public static final int CURRENT_LIMIT = 12;
-
-        public static final double AUTO_MAX_ACCEL_RPSS = MAX_VEL_RPS / 1.0;
-
-        public static final Constraints ARM_CONSTRAINTS = new Constraints(Constants.Arm.MAX_VEL_RPS,
-                Constants.Arm.MAX_ACCEL_RPSS);
-
-        public static final Constraints AUTO_ARM_CONSTRAINTS = new Constraints(Constants.Arm.MAX_VEL_RPS,
-                Constants.Arm.AUTO_MAX_ACCEL_RPSS);
-
-        public static TrapezoidProfile ARM_PROFILE = new TrapezoidProfile(ARM_CONSTRAINTS);
-        public static TrapezoidProfile AUTO_ARM_PROFILE = new TrapezoidProfile(AUTO_ARM_CONSTRAINTS);
+        public static final String armCANbus = "rio";
+
+        // public static final State SCORINGPOS = new State(-33, 0);
+        // public static final double SOFTLIMIT = SCORINGPOS.position - 2;
+        // public static final State INTAKEPOS = new State(0.0, 0);
+        // public static final State CONSTRAINTPOS = new State(-13, 0);
+
+        public static final double scoringPosition = 0.3;
+        public static final double intakePosition = 0;
+        public static final double positionTolerance = 0.01; 
+        public static final double constrainPosition = 0.1;
+
+        public static final double homeSpeed = -0.3;
+
+        public static final class MotorOutput {
+            public static final NeutralModeValue neutralMode = NeutralModeValue.Brake;
+            public static final InvertedValue inversion = InvertedValue.CounterClockwise_Positive;
+        }
+
+        public static final class CurrentLimits {
+            // Current Limit Constants
+            public static final double supplyCurrentLimit = 40.0;
+            public static final boolean supplyCurrentEnable = true;
+            public static final double supplyCurrentThreshold = 50.0;
+            public static final double supplyTimeThreshold = 0.2;
+        }
+
+        public static final class Feedback {
+            // Feedback Constants
+            public static final double sensorToMechanismRatio = 100; // rotor rotations to wrist rotations
+        }
+
+
+
+        public static final class SoftwareLimits {
+            // Software Limit Switch Constants
+            public static SoftwareLimitSwitchConfigs normalSoftLimits = new SoftwareLimitSwitchConfigs();
+            static {
+                normalSoftLimits.ForwardSoftLimitEnable = true;
+                normalSoftLimits.ForwardSoftLimitThreshold = 36; // 108.0/360.0; // rotations
+                normalSoftLimits.ReverseSoftLimitEnable = false;
+                // normalSoftLimits.ReverseSoftLimitThreshold = -1000;
+            }
+        }
+
+        public static final class HardWareLimits {
+            // Hardware Limit Switch Constants
+            public static final ReverseLimitSourceValue reverseLimitSource = ReverseLimitSourceValue.Disabled;
+            public static final ReverseLimitTypeValue reverseLimitType = ReverseLimitTypeValue.NormallyOpen;
+            public static final boolean reverseLimitEnabled = true; // check
+            public static final boolean reverseLimitAutosetPositionEnabled = false; // check
+            public static final double reverseLimitAutosetPositionValue = 0.0;
+            public static final ForwardLimitSourceValue forwardLimitSource = ForwardLimitSourceValue.LimitSwitchPin;
+            public static final ForwardLimitTypeValue forwardLimitType = ForwardLimitTypeValue.NormallyOpen;
+            public static final boolean forwardLimitEnabled = false; // check
+            public static final boolean forwardLimitAutosetPositionEnabled = false; // check
+            public static final double forwardLimitAutosetPositionValue = 0.0;
+        }
     }
 
     public static final class Wrist {
         public static final int WRIST_MOTOR_ID = 11;
         public static final int WRIST_FOLLOW_ID = 12;
 
-        public static final State SCORINGPOS = new State(-23.0, 0);
-        public static final State INTAKEPOS = new State(-47.0, 0);
-        public static final State HOMEPOS = new State(0.0, 0);
-
-        public static final double kP = 0.08;
-        public static final double kI = 0.0;
-        public static final double kD = 0.002;
-        public static final double kIz = 0.0;
-        public static final double kFF = 0.0;
-
-        public static final double kS = 0.3;
-        public static final double kG = 0.21;
-        public static final double kV = 4.9;
-        public static final double kA = 0.01;
-
-        public static final ArmFeedforward FEED_FORWARD = new ArmFeedforward(kS, kG, kV, kA);
-
-        public static final double MAX_OUTPUT = 1.0;
-        public static final double MIN_OUTPUT = -1.0;
-        public static final double MAX_VEL_RPS = 33.0 / 2.0;
-        public static final double MAX_ACCEL_RPSS = MAX_VEL_RPS / 2.0;
-        public static final double ALLOWED_ERROR = 2.0;
-        public static final int CURRENT_LIMIT = 10;
+        public static final String wristCANbus = "rio";
 
-        public static final double AUTO_MAX_ACCEL_RPSS = MAX_VEL_RPS / 1.0;
-
-        public static final Constraints WRIST_CONSTRAINTS = new Constraints(Constants.Wrist.MAX_VEL_RPS,
-                Constants.Wrist.MAX_ACCEL_RPSS);
-
-        public static final Constraints AUTO_WRIST_CONSTRAINTS = new Constraints(Constants.Wrist.MAX_VEL_RPS,
-                Constants.Wrist.AUTO_MAX_ACCEL_RPSS);
+        public static final State SCORINGPOS = new State(0.23, 0);
+        public static final State INTAKEPOS = new State(0.5, 0);
+        public static final State HOMEPOS = new State(0.0, 0);
+        public static final State CONSTRAINTPOS = new State(0.45, 0);
 
-        public static TrapezoidProfile WRIST_PROFILE = new TrapezoidProfile(WRIST_CONSTRAINTS);
-        public static TrapezoidProfile AUTO_WRIST_PROFILE = new TrapezoidProfile(AUTO_WRIST_CONSTRAINTS);
+        public static final double homeSpeed = -0.4;
+        public static final double outputRatio = 100;                                                                    
     }
 
     public static final class Intake {
         public static final int INTAKE_MOTOR_ID = 13;
-        public static final double PERCENT = 1.0;
+        public static final double intakePercent = 1.0;
+        public static final double scorePercent = 0.6;
         public static final int CURRENT_LIMIT = 20;
+        public static final int distanceThreshold = 500;
     }
 
     public static final class Climb {
@@ -211,4 +215,8 @@ public static final class Climb {
         public static final double HALFWAY_POS_ROT = 45.0;
         public static final double TOP_POS_ROT = 90.0;
     }
+
+    public static final class Lights {
+        public static final int CANdleID = 0;
+    }
 }

src/main/java/raidone/robot/Conventions.txt

@@ -0,0 +1,11 @@
+Raid One Code Conventions
+
+When creating buttons, append "Btn" to the name:
+
+    private final JoystickButton wristHomeBtn = new JoystickButton(driver, XboxController.Button.kA.value);
+
+Abbreviate parameters to avoid using "this."
+
+    public static void setRobotState(RobotState rs){
+        robotState = rs;
+    }