Merge branch 'main' of https://github.com/team581/offseason-2023-bot-1

team581 · Aug 26, 2023 · f0dff8d · f0dff8d
2 parents a443146 + ac4722b
commit f0dff8d
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 7 deletions.
diff --git a/src/main/java/frc/robot/intake/IntakeSubsystem.java b/src/main/java/frc/robot/intake/IntakeSubsystem.java
@@ -5,19 +5,26 @@
 package frc.robot.intake;
 
 import com.revrobotics.*;
+
+import edu.wpi.first.math.filter.LinearFilter;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
 import org.littletonrobotics.junction.Logger;
 
 /** Add your docs here. */
 public class IntakeSubsystem extends LifecycleSubsystem {
-  private CANSparkMax motor;
+  private final LinearFilter voltageFilter = LinearFilter.movingAverage(7);
+  private final LinearFilter velocityFilter = LinearFilter.movingAverage(7);
+  //7 is a placeholder not tuned
+  private final CANSparkMax motor;
+  private final RelativeEncoder encoder;
   private IntakeState goalState;
   private HeldGamePiece gamePiece = HeldGamePiece.NOTHING;
-
+  
   public IntakeSubsystem(CANSparkMax motor) {
     super(SubsystemPriority.INTAKE);
     this.motor = motor;
+    encoder = motor.getEncoder();
   }
 
   public void setGoalState(IntakeState intakeState) {
@@ -42,26 +49,34 @@ public void robotPeriodic() {
   public void enabledPeriodic() {
     if (goalState == IntakeState.OUTTAKE_CONE) {
       motor.set(0.4);
-      // gamePiece = HeldGamePiece.NOTHING;
     } else if (goalState == IntakeState.OUTTAKE_CUBE) {
       motor.set(-0.3);
-      // gamePiece = HeldGamePiece.NOTHING;
     } else if (gamePiece == HeldGamePiece.CUBE) {
       motor.set(0.15);
     } else if (gamePiece == HeldGamePiece.CONE) {
       motor.set(-0.1);
     } else if (goalState == IntakeState.INTAKE_CUBE) {
       motor.set(0.5);
-      // gamePiece = HeldGamePiece.CUBE;
     } else if (goalState == IntakeState.INTAKE_CONE) {
       motor.set(-1);
-      // gamePiece = HeldGamePiece.CONE;
     } else {
       motor.disable();
     }
+
+    // Game piece detection
+    double motorVelocity = velocityFilter.calculate(encoder.getVelocity());
+    double intakeVoltage = voltageFilter.calculate(motor.getAppliedOutput()) * 12.0;
+    double theoreticalSpeed = intakeVoltage * (5700.0/12.0); //Neo Max is 5700
+    double threshold = theoreticalSpeed * 0.5; 
+    if (motorVelocity < threshold && goalState == IntakeState.INTAKE_CONE) {
+      gamePiece = HeldGamePiece.CONE;
+    } else if (motorVelocity < threshold && goalState == IntakeState.INTAKE_CUBE) {
+      gamePiece = HeldGamePiece.CUBE;
+    } else if (motorVelocity > threshold && (goalState == IntakeState.OUTTAKE_CONE || goalState == IntakeState.OUTTAKE_CUBE)) {
+      gamePiece = HeldGamePiece.NOTHING;
+    }
   }
 
-  // create an atGoal(IntakeMode mode) method
   public boolean atGoal(IntakeState state) {
     if (goalState != state) {
       return false;

diff --git a/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java
@@ -27,6 +27,8 @@ public ShoulderSubsystem(CANSparkMax motor, CANSparkMax motorFollower) {
     motorFollower.follow(motor);
     encoder = motor.getEncoder();
     pid = motor.getPIDController();
+    motor.setSmartCurrentLimit(35);
+    motorFollower.setSmartCurrentLimit(35);
 
     pid.setP(5);
     pid.setI(0);