made a little Mechanism2d thing for the intake and added the calibrat…

…ion for the intake
DurhamAcademy · Sep 15, 2023 · 5882b14 · 5882b14
1 parent d719fc0
commit 5882b14
Show file tree

Hide file tree

Showing 9 changed files with 134 additions and 31 deletions.
diff --git a/.gitignore b/.gitignore
@@ -165,6 +165,8 @@ simgui*.json
 
 # Version file
 src/main/java/frc/robot/BuildConstants.java
+
+# Others
 /*.wpilog
 /.idea/vcs.xml
 /.idea/gradle.xml
@@ -175,4 +177,4 @@ src/main/java/frc/robot/BuildConstants.java
 /networktables.json
 /.idea/modules/RewriteFRC2023.main.iml
 /.idea/modules/RewriteFRC2023.test.iml
-/.idea/compiler.xml
+/.idea/compiler.xml
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -15,7 +15,7 @@
  * wherever the constants are needed, to reduce verbosity.
  */
 public final class Constants {
-    public static final Mode currentMode = Mode.REAL;
+    public static final Mode currentMode = Mode.SIM;
     public static double loopPeriodSecs = 0.02;
 
     public enum Mode {

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -24,14 +24,19 @@
  */
 public class Robot extends LoggedRobot {
   private Command autonomousCommand;
+  private Command startCommand;
+
   private RobotContainer robotContainer;
 
   /**
    * This function is run when the robot is first started up and should be used
    * for any initialization code.
    */
+  @SuppressWarnings("DataFlowIssue")
   @Override
   public void robotInit() {
+
+
     Logger logger = Logger.getInstance();
 
     // Record metadata
@@ -89,6 +94,9 @@ public void robotInit() {
     } catch (IOException e) {
       throw new RuntimeException(e);
     }
+
+
+    startCommand = robotContainer.getStartCommand();
   }
 
   /** This function is called periodically during all modes. */
@@ -123,7 +131,7 @@ public void autonomousInit() {
 
     // schedule the autonomous command (example)
     if (autonomousCommand != null) {
-      autonomousCommand.schedule();
+      startCommand.andThen(autonomousCommand).schedule();
     }
   }
 
@@ -142,6 +150,8 @@ public void teleopInit() {
     if (autonomousCommand != null) {
       autonomousCommand.cancel();
     }
+
+    startCommand.schedule();
   }
 
   /** This function is called periodically during operator control. */

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.SpinAuto;
+import frc.robot.commands.ZeroModeMotor;
 import frc.robot.subsystems.battery.BatteryIO;
 import frc.robot.subsystems.battery.BatteryIOInputsAutoLogged;
 import frc.robot.subsystems.battery.BatteryIORio;
@@ -38,6 +39,7 @@
 public class RobotContainer {
     // Subsystems
     private final Drive drive;
+    @SuppressWarnings("FieldCanBeLocal")
     private final Intake intake;
 //    private final Flywheel flywheel;
 
@@ -50,6 +52,8 @@ public class RobotContainer {
     // Battery IO
     BatteryIO battery;
     BatteryIOInputsAutoLogged batteryInputs = new BatteryIOInputsAutoLogged();
+    ZeroModeMotor zeroModeMotor;
+
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
@@ -129,6 +133,7 @@ public RobotContainer() throws IOException {
 
         // Configure the button bindings
         configureButtonBindings();
+        zeroModeMotor = new ZeroModeMotor(intake);
     }
 
     /**
@@ -157,4 +162,8 @@ public void periodic() {
         this.battery.updateInputs(batteryInputs);
         Logger.getInstance().processInputs("Battery", batteryInputs);
     }
+
+    public Command getStartCommand() {
+        return zeroModeMotor;
+    }
 }
diff --git a/src/main/java/frc/robot/commands/ZeroModeMotor.java b/src/main/java/frc/robot/commands/ZeroModeMotor.java
@@ -0,0 +1,34 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.intake.Intake;
+
+public class ZeroModeMotor extends CommandBase {
+
+    Timer timer = new Timer();
+    Intake intake;
+
+    public ZeroModeMotor(Intake intake) {
+        addRequirements(intake);
+        this.intake = intake;
+    }
+
+    public void initialize() {
+        timer.restart();
+        intake.startModeMotorZeroing();
+    }
+
+    public void execute() {
+        intake.setZeroModeVoltage(-3.0);
+    }
+
+    public void end(boolean interrupted) {
+        intake.endModeMotorZeroing();
+    }
+
+    public boolean isFinished() {
+        return timer.hasElapsed(1.0);
+    }
+
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -2,7 +2,12 @@
 
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import org.littletonrobotics.junction.Logger;
 
 import static edu.wpi.first.math.MathUtil.clamp;
@@ -31,8 +36,13 @@ public class Intake extends SubsystemBase {
                     32.0
             )
     );
-    double modeVoltage = 0.0;
-    boolean modeZeroed = false;
+    private final MechanismLigament2d mechanismDeployArm = new MechanismLigament2d("Deploy Arm", 0.6, 90.0);
+    private final Mechanism2d mechanism2d = new Mechanism2d(5.0, 5.0);
+    private final MechanismLigament2d mechanismModeArm = new MechanismLigament2d("Mode Arm", 0.2, 170.0);
+    private final CommandXboxController controller = new CommandXboxController(0);
+    private double modeVoltage = 0.0;
+    private boolean modeZeroed = false;
+
     public Intake(IntakeIO intakeIO) {
         this.intakeIO = intakeIO;
         inputs = new IntakeIOInputsAutoLogged();
@@ -47,6 +57,24 @@ public Intake(IntakeIO intakeIO) {
                 0.0,
                 0.0
         );
+
+        MechanismRoot2d mechanismRoot = mechanism2d.getRoot("Intake", 2.5, 2.5);
+        mechanismRoot.append(mechanismDeployArm);
+        mechanismDeployArm.append(mechanismModeArm);
+    }
+
+    public void setZeroModeVoltage(double voltage) {
+        modeVoltage = voltage;
+    }
+
+    public void startModeMotorZeroing() {
+        modeZeroed = false;
+    }
+
+    public void endModeMotorZeroing() {
+        intakeIO.setModeEncoderPosition(0.0);
+        modeZeroed = true;
+        setZeroModeVoltage(0.0);
     }
 
     @Override
@@ -57,15 +85,20 @@ public void periodic() {
 
         var deployVoltage = deployPID.calculate(
                 inputs.deployEncoderPosition,
-                0.0
+                controller.getLeftX()
         );
 
         var modeV = (modeZeroed) ? modePID.calculate(
                 inputs.modeEncoderPosition,
-                0.0
+                controller.getLeftY() * 2 * PI
         ) : modeVoltage;
 
         intakeIO.setDeployVoltage(clamp(deployVoltage, -6.0, 6.0));
         intakeIO.setModeVoltage(clamp(modeV, -3.0, 3.0));
+
+        mechanismModeArm.setAngle(Units.radiansToDegrees(inputs.modeEncoderPosition) - 160.0);
+        mechanismDeployArm.setAngle(Units.radiansToDegrees(inputs.deployEncoderPosition) + 90.0);
+        Logger.getInstance().recordOutput("Intake", mechanism2d);
     }
+
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java
@@ -1,32 +1,43 @@
 package frc.robot.subsystems.intake;
 
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import frc.robot.Constants;
 
 import static java.lang.Math.PI;
 
 public class IntakeIOSim implements IntakeIO {
     SingleJointedArmSim deploySim = new SingleJointedArmSim(DCMotor.getNEO(1),
-            1 / 20.0,
-            5.0,
-            .5,
+            /*1 /*/ 20.0,
+            0.1,
+            .8,
             -PI / 4,
             PI / 2,
             true);
 
     SingleJointedArmSim modeSim = new SingleJointedArmSim(DCMotor.getNeo550(1),
-            1 / 50.0,
-            5.0,
-            .5,
+            /*1 /*/ 50.0,
+            0.1,
+            .2,
             -PI / 2,
             3 * PI / 4,
             true);
 
+    public IntakeIOSim() {
+        modeSim.setInput(2.0);
+    }
+
     @Override
     public void updateInputs(IntakeIOInputs inputs) {
+        if (RobotState.isDisabled()) {
+            modeSim.setInputVoltage(0.0);
+            deploySim.setInputVoltage(0.0);
+        }
+
         deploySim.update(Constants.loopPeriodSecs);
         modeSim.update(Constants.loopPeriodSecs);
+
         inputs.modeMotorOutputCurrent = modeSim.getCurrentDrawAmps();
         inputs.deployMotorOutputCurrent = deploySim.getCurrentDrawAmps();
         inputs.modeEncoderPosition = modeSim.getAngleRads();

diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java
@@ -3,6 +3,7 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkMaxAbsoluteEncoder;
+import edu.wpi.first.math.util.Units;
 
 import static com.revrobotics.CANSparkMax.IdleMode.kBrake;
 import static com.revrobotics.CANSparkMaxLowLevel.MotorType.kBrushless;
@@ -54,7 +55,7 @@ public IntakeIOSparkMax() {
 
     @Override
     public void updateInputs(IntakeIOInputs inputs) {
-        inputs.deployEncoderPosition = deployEncoder.getPosition();
+        inputs.deployEncoderPosition = Units.rotationsToRadians(deployEncoder.getPosition());
         inputs.deployMotorOutputCurrent = deployMotor.getOutputCurrent();
 
         inputs.intakeMotorSpeed = intakeMotor.getEncoder().getVelocity();

diff --git a/src/test/java/frc/robot/subsystems/drive/DriveTest.java b/src/test/java/frc/robot/subsystems/drive/DriveTest.java
@@ -1,7 +1,10 @@
 package frc.robot.subsystems.drive;
 
 import edu.wpi.first.wpilibj.Timer;
-import org.junit.jupiter.api.*;
+import org.junit.jupiter.api.Assertions;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.DisplayName;
+import org.junit.jupiter.api.Test;
 
 import java.io.IOException;
 import java.util.Random;
@@ -129,20 +132,20 @@ void getPoseY() {
         Assertions.assertNotEquals(0.0, drive.getPose().getY());
     }
 
-    @Test
-    @Disabled("Rotation does not work")
-    @DisplayName("getPose() Rotation Getter Test")
-    void getPoseRot() {
-        Assertions.assertEquals(0.0, drive.getPose().getRotation().getRadians());
-
-        drive.testMode = true;
-        drive.testTime = Timer.getFPGATimestamp();
-        for (int i = 0; i < 50; i++) {
-            drive.periodic();
-            drive.testTime += 0.02;
-        }
-
-        System.out.println(drive.getPose());
-        Assertions.assertNotEquals(0.0, drive.getPose().getRotation().getRadians());
-    }
+//    @Test
+//    @Disabled("Rotation does not work")
+//    @DisplayName("getPose() Rotation Getter Test")
+//    void getPoseRot() {
+//        Assertions.assertEquals(0.0, drive.getPose().getRotation().getRadians());
+//
+//        drive.testMode = true;
+//        drive.testTime = Timer.getFPGATimestamp();
+//        for (int i = 0; i < 50; i++) {
+//            drive.periodic();
+//            drive.testTime += 0.02;
+//        }
+//
+//        System.out.println(drive.getPose());
+//        Assertions.assertNotEquals(0.0, drive.getPose().getRotation().getRadians());
+//    }
 }