workshop 10/8

src/main/java/frc/team3128/Constants.java

@@ -275,7 +275,7 @@ public static class ShooterConstants {
         public static final double MIDDLE_FEED_ANGLE = 25;
 
         public static final double INTAKE_POWER = 0.65;
-        public static final double KICK_POWER = 0.5;
+        public static final double KICK_POWER = 0.4;
         public static final double KICK_SHOOTING_POWER = 0.8;
         public static final double CURRENT_TEST_POWER = 0;
         public static final double CURRENT_TEST_PLATEAU = 0;
@@ -312,7 +312,7 @@ public static class IntakeConstants {
         public static final double STALL_CURRENT = 50;
         public static final double STALL_POWER = .05;
         public static final double OUTTAKE_POWER = -0.3;
-        public static final double INTAKE_POWER = .65;
+        public static final double INTAKE_POWER = .8;
         public static final double VOLT_COMP = 9;
     }
 
@@ -439,7 +439,7 @@ public static class AmperConstants {
         public static final double WHEEL_CIRCUMFERENCE = Units.inchesToMeters(0.9023) * Math.PI;
         public static final double UNIT_CONV_FACTOR = GEAR_RATIO * WHEEL_CIRCUMFERENCE * 100;
 
-        public static final double ROLLER_POWER = 0.5;
+        public static final double ROLLER_POWER = 0.8;
 
         public static final double AMPER_ANGLE = 31.96;
     }

src/main/java/frc/team3128/Robot.java

@@ -10,6 +10,7 @@
 
 import common.core.misc.NAR_Robot;
 import common.hardware.camera.Camera;
+import common.hardware.motorcontroller.NAR_Motor.Neutral;
 import common.utility.Log;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.DriverStation.MatchType;
@@ -25,6 +26,7 @@
 import frc.team3128.commands.CmdManager;
 import frc.team3128.subsystems.Hopper;
 import frc.team3128.subsystems.Leds;
+import frc.team3128.subsystems.Shooter;
 import frc.team3128.subsystems.Swerve;
 
 /**

src/main/java/frc/team3128/RobotContainer.java

@@ -44,6 +44,7 @@
 import common.utility.shuffleboard.NAR_Shuffleboard;
 import common.utility.sysid.CmdSysId;
 import common.utility.tester.Tester;
+import frc.team3128.subsystems.Amper;
 import frc.team3128.subsystems.Hopper;
 import frc.team3128.subsystems.Intake;
 // import common.utility.tester.Tester.UnitTest;
@@ -62,6 +63,7 @@
 public class RobotContainer {
 
     private Swerve swerve;
+    private Amper amper;
     private Hopper hopper;
     private Intake intake;
     private Shooter shooter;
@@ -71,6 +73,7 @@ public class RobotContainer {
     private NAR_ButtonBoard buttonPad;
 
     public static NAR_XboxController controller;
+    public static NAR_XboxController controller2;
 
     private NarwhalDashboard dashboard;
 
@@ -87,8 +90,10 @@ public RobotContainer() {
         // judgePad = new NAR_ButtonBoard(1);
         controller = new NAR_XboxController(2);
         buttonPad = new NAR_ButtonBoard(3);
+        controller2 = new NAR_XboxController(4);
 
         swerve = Swerve.getInstance();
+        amper = Amper.getInstance();
         hopper = Hopper.getInstance();
         intake = Intake.getInstance();
         shooter = Shooter.getInstance();
@@ -128,8 +133,11 @@ private void configureButtonBindings() {
             CmdSwerveDrive.setTurnSetpoint(Robot.getAlliance() == Alliance.Red ? 270 : 90);
         }));
 
+        controller.getButton(XboxButton.kStart).onTrue(runOnce(()-> swerve.zeroGyro(0)));
+
         controller.getButton(XboxButton.kLeftTrigger).onTrue(intake(Intake.Setpoint.GROUND));
         controller.getButton(XboxButton.kLeftBumper).onTrue(retractIntake());
+
         controller.getButton(XboxButton.kRightTrigger).onTrue(shooter.rampUpShooter()).onFalse(shooter.setShooting(true));
 
         controller.getButton(XboxButton.kA).onTrue(shooter.runShooter(0.8));
@@ -138,6 +146,13 @@ private void configureButtonBindings() {
 
         controller.getButton(XboxButton.kY).whileTrue(ampUp()).onFalse(ampFinAndDown());
 
+        controller2.getButton(XboxButton.kA).onTrue(runOnce(()-> intake.disable()).andThen(intake.reset(0)));
+        controller2.getButton(XboxButton.kB).onTrue(runOnce(()-> amper.disable()).andThen(amper.reset(0)));
+        controller2.getButton(XboxButton.kRightTrigger).onTrue(intake.runPivot(0.3));
+        controller2.getButton(XboxButton.kRightBumper).onTrue(intake.runPivot(-0.3));
+
+
+
         // new Trigger(()->true).onTrue(queueNote());
 
         //Shooting
@@ -152,8 +167,9 @@ private void configureButtonBindings() {
         ));
 
         //Stops shooting when all notes are gone
-        new Trigger(()-> shooter.noteInRollers()).negate()
-        .and(()->hopper.hasObjectPresent()).negate()
+        new Trigger(()-> !shooter.noteInRollers())
+        .and(()-> !hopper.hasObjectPresent())
+        .debounce(0.5)
         .onTrue(sequence(
             shooter.setShooting(false),
             runOnce(() -> leds.setLedColor(Colors.BLUE))
@@ -184,12 +200,25 @@ private void configureButtonBindings() {
             waitSeconds(.1),
             shooter.runKickMotor(0)
         ));
+
+        // new Trigger(()-> !shooter.noteInRollers())
+        // .debounce(0.25)
+        // .onTrue(amper.retract());
 
         // new Trigger(() -> shouldEjectNote()).onTrue(sequence(
         //     runOnce(() -> leds.setLedColor(Colors.PURPLE)),
         //     ejectNote()
         //     ));
 
+        new Trigger(()-> amper.getMeasurement() > 3)
+        .onTrue(amper.runRollers())
+        .onFalse(amper.stopRollers());
+
+        new Trigger(()-> amper.getMeasurement() > 3)
+        .and(()-> !shooter.noteInRollers())
+        .debounce(2)
+        .onTrue(amper.retract());
+
     }
 
     private Timer ejecTimer = new Timer();

src/main/java/frc/team3128/commands/CmdManager.java

@@ -169,10 +169,11 @@ public static Command ampFinAndDown(){
         return sequence(
             amper.fullExtend(),
             waitUntil(() -> amper.atSetpoint()),
-            shooter.setShooting(true),
-            waitUntil(() -> !shooter.getShooting()),
-            amper.stopRollers(),
-            amper.retract()
+            shooter.setShooting(true)
+            // waitUntil(() -> !shooter.getShooting())
+            // waitSeconds(1),
+            // amper.runRollers(0),
+            // amper.moveElevator(0)
         );
     }
 

src/main/java/frc/team3128/subsystems/Amper.java

@@ -53,8 +53,9 @@ protected void configMotors() {
         ELEV_MOTOR.setUnitConversionFactor(UNIT_CONV_FACTOR);
         ELEV_MOTOR.setCurrentLimit(CURRENT_LIMIT);
         ELEV_MOTOR.setInverted(true);
+        ROLLER_MOTOR.setInverted(true);
 
-        ELEV_MOTOR.setNeutralMode(Neutral.COAST);
+        ELEV_MOTOR.setNeutralMode(Neutral.BRAKE);
         ROLLER_MOTOR.setNeutralMode(Neutral.COAST);
 
         ELEV_MOTOR.setStatusFrames(SparkMaxConfig.POSITION);

src/main/java/frc/team3128/subsystems/Intake.java

@@ -6,6 +6,7 @@
 import common.core.subsystems.PivotTemplate;
 import common.hardware.motorcontroller.NAR_CANSpark.SparkMaxConfig;
 import common.hardware.motorcontroller.NAR_Motor.Neutral;
+import common.utility.shuffleboard.NAR_Shuffleboard;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -16,9 +17,8 @@
 public class Intake extends PivotTemplate{
 
     public enum Setpoint {
-        GROUND(MAX_SETPOINT),
-        SOURCE(60),
-        NEUTRAL(MIN_SETPOINT);
+        GROUND(138),
+        NEUTRAL(0);
 
 
         public final double angle;
@@ -67,12 +67,20 @@ protected void configMotors() {
     public Command retract() {
         return sequence(
             pivotTo(Setpoint.NEUTRAL),
-            waitUntil(()-> atSetpoint()).withTimeout(1.5),
-            runPivot(-0.2),
-            waitSeconds(0.1),
-            runPivot(0),
-            reset(0)
-        );
+            runOuttakeRollers(),
+            waitUntil(()-> atSetpoint()).withTimeout(1.5)
+            // reset(0)//,
+            // runPivot(-0.2),
+            // waitSeconds(0.1),
+            // runPivot(0),
+            // reset(0)
+        ).andThen(stopRollers());
+    }
+
+    @Override
+    public void initShuffleboard(){
+        super.initShuffleboard();
+        NAR_Shuffleboard.addData(getName(), "Current", ()-> ROLLER_MOTOR.getStallCurrent(), 4, 0);
     }
 
     @Override

src/main/java/frc/team3128/subsystems/Swerve.java

@@ -103,7 +103,7 @@ public double getRoll() {
     @Override
     public void zeroGyro(double reset) {
         // gyro.setYaw(reset);
-        gyroOffset = getYaw();
+        gyroOffset = (Robot.getAlliance() == Alliance.Red ? 180 : 0) - gyro.getAngle();
     }
 
     public double getPredictedDistance() {