Feature/intake phx6 alon

build.gradle

@@ -101,7 +101,7 @@ dependencies {
 
     implementation group: 'com.google.code.gson', name: 'gson', version: '2.8.6'
 
-    implementation 'com.github.galaxia5987:common:1181c49'
+    implementation 'com.github.galaxia5987:common:3580ac1cfd'
 }
 
 test {

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

@@ -9,6 +9,7 @@
 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;

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

@@ -4,6 +4,7 @@
 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;
@@ -16,18 +17,26 @@
 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.IntakeKeyboardControl;
 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;
 
+
 public class RobotContainer {
     private static RobotContainer INSTANCE = null;
     private final Arm arm = Arm.getINSTANCE();
     private final Leds leds = Leds.getInstance();
-    private final SwerveDrive swerveDrive = SwerveDrive.getInstance(Robot.isReal());
+    static {
+        SwerveDrive.setInstance(Robot.isReal(),
+                Ports.SwerveDrive.DRIVE_IDS,
+                Ports.SwerveDrive.ANGLE_IDS,
+                Ports.SwerveDrive.ENCODER_IDS);
+    }
+    private final SwerveDrive swerveDrive = SwerveDrive.getInstance();
     private final Intake intake = Intake.getInstance();
     private final Gripper gripper = Gripper.getInstance();
     private final XboxController xboxController = new XboxController(0);
@@ -77,35 +86,12 @@ private void configureDefaultCommands() {
                 new XboxDrive(swerveDrive, xboxController)
         );
 //        arm.setDefaultCommand(new ArmXboxControl(xboxController));
-        intake.setDefaultCommand(new HoldIntakeInPlace());
+        intake.setDefaultCommand(new IntakeKeyboardControl());
         leds.setDefaultCommand(new ProximitySensorDefaultCommand());
     }
 
     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());
-
-        rb.whileTrue(new ArmAxisControl(0.02, 0.02)
-                .until(() -> gripper.getDistance() < ArmConstants.FEEDER_DISTANCE));
+      //  a.whileTrue(new RunCommand(()-> intake.setAngleMotorAngle(Math.PI)));
     }
 
     /**

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

@@ -7,7 +7,7 @@
 
 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

@@ -9,7 +9,7 @@
 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/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 utils.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;
 
 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;

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

@@ -1,11 +1,13 @@
 package frc.robot.subsystems.intake;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+
 public interface IntakeIO {
     void updateInputs(IntakeLoggedInputs inputs);
 
     void setSpinMotorPower(double power);
 
-    void setAngleMotorAngle(double angle);
+    void setAngleMotorAngle(Rotation2d angle);
 
     void setAngleMotorPower(double power);