Feature/intake phx6 alon

build.gradle

@@ -1,5 +1,3 @@
-import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO;
-
 plugins {
     id "java"
     id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3"
@@ -100,8 +98,7 @@ dependencies {
     implementation 'com.sparkjava:spark-core:2.9.4'
 
     implementation group: 'com.google.code.gson', name: 'gson', version: '2.8.6'
-
-    implementation 'com.github.galaxia5987:common:1181c49'
+    implementation files('libs/common.jar')
 }
 
 test {
@@ -141,10 +138,10 @@ tasks.withType(JavaCompile) {
 // Create version file
 project.compileJava.dependsOn(createVersionFile)
 gversion {
-    srcDir       = "src/main/java/"
+    srcDir = "src/main/java/"
     classPackage = "frc.robot"
-    className    = "BuildConstants"
-    dateFormat   = "yyyy-MM-dd HH:mm:ss z"
-    timeZone     = "America/New_York"
-    indent       = "  "
+    className = "BuildConstants"
+    dateFormat = "yyyy-MM-dd HH:mm:ss z"
+    timeZone = "America/New_York"
+    indent = "  "
 }

src/main/java/frc/robot/Robot.java

@@ -9,13 +9,14 @@
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.intake.Intake;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
-import utils.math.differential.BooleanTrigger;
+import lib.math.differential.BooleanTrigger;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to

src/main/java/frc/robot/RobotContainer.java

@@ -3,33 +3,36 @@
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.commandgroups.PickUpCubeTeleop;
-import frc.robot.commandgroups.ReturnIntake;
 import frc.robot.subsystems.arm.Arm;
-import frc.robot.subsystems.arm.ArmConstants;
-import frc.robot.subsystems.arm.ArmPosition;
-import frc.robot.subsystems.arm.commands.ArmAxisControl;
-import frc.robot.subsystems.arm.commands.ArmWithSpline;
 import frc.robot.subsystems.gripper.Gripper;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.intake.commands.HoldIntakeInPlace;
 import frc.robot.subsystems.intake.commands.ProximitySensorDefaultCommand;
 import frc.robot.subsystems.intake.commands.Retract;
 import frc.robot.subsystems.leds.Leds;
-import swerve.SwerveDrive;
-import swerve.commands.XboxDrive;
-import utils.Utils;
+import frc.robot.swerve.SwerveDrive;
+import frc.robot.swerve.commands.XboxDrive;
+import lib.Utils;
+
 
 public class RobotContainer {
     private static RobotContainer INSTANCE = null;
-    private final Arm arm = Arm.getINSTANCE();
+
+    static {
+        SwerveDrive.setInstance(Robot.isReal(),
+                Ports.SwerveDrive.DRIVE_IDS,
+                Ports.SwerveDrive.ANGLE_IDS,
+                Ports.SwerveDrive.ENCODER_IDS);
+    }
+
+//    private final Arm arm = Arm.getINSTANCE();
     private final Leds leds = Leds.getInstance();
-    private final SwerveDrive swerveDrive = SwerveDrive.getInstance(Robot.isReal());
+    private final SwerveDrive swerveDrive = SwerveDrive.getInstance();
     private final Intake intake = Intake.getInstance();
-    private final Gripper gripper = Gripper.getInstance();
+//    private final Gripper gripper = Gripper.getInstance();
     private final XboxController xboxController = new XboxController(0);
     private final Joystick leftJoystick = new Joystick(1);
     private final Joystick rightJoystick = new Joystick(2);
@@ -82,30 +85,11 @@ private void configureDefaultCommands() {
     }
 
     private void configureButtonBindings() {
-        leftJoystickTrigger.onTrue(new InstantCommand(swerveDrive::resetGyro));
-        y.whileTrue(new ArmWithSpline(ArmPosition.TOP_SCORING));
-        x.whileTrue(new ArmWithSpline(ArmPosition.MIDDLE_SCORING));
-        b.whileTrue(new ArmWithSpline(ArmPosition.FEEDER));
-        back.whileTrue(new ArmWithSpline(ArmPosition.FEEDER_CONE));
-        a.whileTrue(new ArmWithSpline(ArmPosition.NEUTRAL));
-
-        lb.onTrue(new InstantCommand(gripper::toggle));
-
-        start.onTrue(new InstantCommand(leds::toggle));
-
-        leftJoystickTopBottom.toggleOnTrue(
-                new InstantCommand(swerveDrive::lock, swerveDrive)
-        );
-        leftJoystickTopBottom.onTrue(
-                new InstantCommand(leds::toggleRainbow)
-        );
-
-        xboxLeftTrigger.whileTrue(new PickUpCubeTeleop())
-                .onFalse(new Retract(Retract.Mode.UP).andThen(new InstantCommand(() -> intake.setSpinMotorPower(0))));
-        xboxRightTrigger.whileTrue(new ReturnIntake());
+        a.whileTrue(new RunCommand(()-> intake.setSpinMotorPower(0.5)));
+        b.whileTrue(new RunCommand(()-> intake.setSpinMotorPower(-0.5)));
+        x.whileTrue(new RunCommand(()-> intake.setAnglePower(0.5)));
+        y.whileTrue(new RunCommand(()-> intake.setAnglePower(-0.5)));
 
-        rb.whileTrue(new ArmAxisControl(0.02, 0.02)
-                .until(() -> gripper.getDistance() < ArmConstants.FEEDER_DISTANCE));
     }
 
     /**

src/main/java/frc/robot/commandgroups/FeedTeleop.java

@@ -1,5 +1,6 @@
 package frc.robot.commandgroups;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc.robot.subsystems.intake.Intake;
@@ -12,7 +13,7 @@ public FeedTeleop(boolean outtake) {
 
         addCommands(
                 new Retract(Retract.Mode.DOWN)
-                        .andThen(new RunCommand(() -> intake.setAngleMotorAngle(IntakeConstants.ANGLE_DOWN))),
+                        .andThen(new RunCommand(() -> intake.setAngleMotorAngle(Rotation2d.fromDegrees(IntakeConstants.ANGLE_DOWN)))),
                 intake.run(outtake ? -IntakeConstants.INTAKE_POWER : IntakeConstants.INTAKE_POWER)
         );
     }

src/main/java/frc/robot/commandgroups/bits/CheckSwerve.java

@@ -3,11 +3,11 @@
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.Robot;
-import swerve.SwerveDrive;
+import frc.robot.swerve.SwerveDrive;
 
 public class CheckSwerve extends SequentialCommandGroup {
     public CheckSwerve() {
-        SwerveDrive swerveDrive = SwerveDrive.getInstance(Robot.isReal());
+        SwerveDrive swerveDrive = SwerveDrive.getInstance();
         addCommands(
                 new RunCommand(swerveDrive::checkSwerve, swerveDrive).withTimeout(5),
                 new ZeroPositionSwerve().withTimeout(2)

src/main/java/frc/robot/commandgroups/bits/ZeroPositionSwerve.java

@@ -4,12 +4,12 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Robot;
-import swerve.SwerveDrive;
+import frc.robot.swerve.SwerveDrive;
 
 import java.util.Arrays;
 
 public class ZeroPositionSwerve extends Command {
-    private final SwerveDrive swerve = SwerveDrive.getInstance(Robot.isReal());
+    private final SwerveDrive swerve = SwerveDrive.getInstance();
     private final Timer timer = new Timer();
     private final SwerveModuleState[] zeroStates = new SwerveModuleState[4];
 

src/main/java/frc/robot/subsystems/arm/ArmKinematics.java

@@ -1,7 +1,7 @@
 package frc.robot.subsystems.arm;
 
 import edu.wpi.first.math.geometry.Translation2d;
-import utils.math.AngleUtil;
+import lib.math.AngleUtil;
 
 /**
  * This class contains the kinematics for the arm.

src/main/java/frc/robot/subsystems/arm/ArmPosition.java

@@ -2,8 +2,8 @@
 
 import edu.wpi.first.math.geometry.Translation2d;
 import frc.robot.subsystems.leds.Leds;
-import utils.math.Vector2;
-import utils.math.spline.QuinticBezierSpline;
+import lib.math.Vector2;
+import lib.math.spline.QuinticBezierSpline;
 
 public enum ArmPosition {
 

src/main/java/frc/robot/subsystems/arm/commands/ArmWithSpline.java

@@ -5,8 +5,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.arm.Arm;
 import frc.robot.subsystems.arm.ArmPosition;
-import utils.math.Vector2;
-import utils.math.spline.QuinticBezierSpline;
+import lib.math.Vector2;
+import lib.math.spline.QuinticBezierSpline;
 
 public class ArmWithSpline extends Command {
 

src/main/java/frc/robot/subsystems/arm/commands/ArmXboxControl.java

@@ -5,8 +5,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Robot;
 import frc.robot.subsystems.arm.Arm;
-import utils.Utils;
-import utils.math.differential.BooleanTrigger;
+import lib.Utils;
+import lib.math.differential.BooleanTrigger;
 
 public class ArmXboxControl extends Command {
     private final Arm arm = Arm.getINSTANCE();

src/main/java/frc/robot/subsystems/intake/Intake.java

@@ -2,6 +2,7 @@
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
@@ -14,6 +15,7 @@
 import frc.robot.Robot;
 import frc.robot.subsystems.intake.commands.HoldIntakeInPlace;
 import org.littletonrobotics.junction.Logger;
+import lib.Utils;
 
 public class Intake extends SubsystemBase {
     private static Intake INSTANCE;
@@ -70,26 +72,26 @@ public void setSpinMotorPower(double power) {
         inputs.setpointSpinMotorPower = power;
     }
 
-    public double getAngleMotorAngle() {
+    public Rotation2d getAngleMotorAngle() {
         return inputs.angleMotorAngle;
     }
 
     /**
      * Sets the angles position.
      *
-     * @param angle is the angle of the retractor. [degrees]
+     * @param angleMotorAngle is the angle of the retractor. [degrees]
      */
-    public void setAngleMotorAngle(double angle) {
-        inputs.setpointAngleMotorAngle = Math.toRadians(angle);
+    public void setAngleMotorAngle(Rotation2d angleMotorAngle) {
+        inputs.setpointAngleMotorAngle = angleMotorAngle;
         angleMode = ControlMode.Position;
     }
 
-    private double getAngleMotorVelocity() {
+    private Rotation2d getAngleMotorVelocity() {
         return inputs.angleMotorVelocity;
     }
 
     public double getCurrent() {
-        return inputs.angleMotorcurrent;
+        return inputs.angleMotorAppliedCurrent;
     }
 
     public void setAnglePower(double power) {
@@ -103,9 +105,12 @@ public void resetEncoder(double angle) {
 
     public Command lowerIntake() {
         return new InstantCommand(() -> resetEncoder(IntakeConstants.ANGLE_UP), this)
-                .andThen(new RunCommand(() -> setAngleMotorAngle(-40), this));
+                .andThen(new RunCommand(() -> setAngleMotorAngle(Rotation2d.fromDegrees(-40)), this));
     }
 
+    // You Jerk :) 😎
+    // I'm gonna kill them all.
+    // All the juice must die
     public Command run(double power) {
         return new RunCommand(() -> this.setSpinMotorPower(power));
     }
@@ -127,9 +132,11 @@ public void periodic() {
                 !(lastCommand instanceof HoldIntakeInPlace);
         lastCommand = currentCommand;
 
-        intakeP1.setAngle(Math.toDegrees(getAngleMotorAngle()) + IntakeConstants.INTAKE_MECH_OFFSET);
+        intakeP1.setAngle(getAngleMotorAngle().getDegrees() + IntakeConstants.INTAKE_MECH_OFFSET);
 
-        Logger.recordOutput("IntakePose", getPose3d(getAngleMotorAngle() + IntakeConstants.INTAKE_SIM_ANGLE_OFFSET));
+
+        var intake3d = getPose3d(getAngleMotorAngle().getRadians() + Math.toRadians(IntakeConstants.INTAKE_SIM_ANGLE_OFFSET)); //TODO: ??!?!! check
+        Logger.recordOutput("IntakePose", Utils.pose3dToArray(intake3d));
         Logger.processInputs("Intake", inputs);
         SmartDashboard.putData("IntakeMech", mech);
     }

src/main/java/frc/robot/subsystems/intake/IntakeConstants.java

@@ -1,15 +1,17 @@
 package frc.robot.subsystems.intake;
 
+import com.ctre.phoenix6.configs.Slot0Configs;
 import edu.wpi.first.math.geometry.Translation2d;
-import utils.units.Units;
+import lib.units.Units;
 
 public class IntakeConstants {
     public static final double ANGLE_GEAR_RATIO = 35.26;
     public static final double SPIN_GEAR_RATIO = 10; //Not a real value
-    public static final double kP = 2;
+    public static final double kP = 0.1*ANGLE_GEAR_RATIO;
     public static final double kI = 0;
-    public static final double kD = 0.2;
-    public static final double kF = 0;
+    public static final double kD = 0;
+    public static final double kF = 0; // not relevant anymore
+    public static final Slot0Configs PIDGains = new Slot0Configs().withKP(IntakeConstants.kP).withKI(IntakeConstants.kI).withKD(IntakeConstants.kD);
 
     public static final double FALCON_TICKS_PER_ROTATION = 2048;
     public static final double TICKS_PER_DEGREE = FALCON_TICKS_PER_ROTATION / 360 * ANGLE_GEAR_RATIO;