Merge branch 'master' into auto-placement

src/main/java/frc4388/robot/RobotContainer.java

@@ -10,7 +10,11 @@ package frc4388.robot;
 import java.util.function.Consumer;
 
 import edu.wpi.first.math.geometry.Translation2d;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.REVLibError;
+import com.revrobotics.CANSparkMax.FaultID;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -54,7 +58,7 @@ public class RobotContainer {
     
     public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
 
-    public final Claw m_robotClaw = new Claw(m_robotMap.servo);
+    public final Claw m_robotClaw = new Claw(m_robotMap.leftClaw, m_robotMap.rightClaw, m_robotMap.spinnyspin);
 
     public final Limelight m_robotLimeLight = new Limelight();
 
@@ -283,6 +287,12 @@ public class RobotContainer {
         // kill soft limits
         new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final
             .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
+
+        // SmartDashboard.putBoolean("isn't SparkMAX", );
+
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
+            .onTrue  (new InstantCommand(() -> m_robotClaw.yesspinnyspin()))
+            .onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
         
         //Arm to Home
         new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final

src/main/java/frc4388/robot/RobotMap.java

@@ -11,8 +11,11 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.WPI_Pigeon2;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.PWM;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc4388.robot.Constants.ArmConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.subsystems.SwerveModule;
@@ -168,6 +171,7 @@ public class RobotMap {
   }
 
   // claw stuff (WHAT IS A SOAP ENGINEER)
-  PWM servo = new PWM(0);
-
-}
+  PWM leftClaw           = new PWM(0);
+  PWM rightClaw          = new PWM(1);
+  CANSparkMax spinnyspin = new CANSparkMax(18, MotorType.kBrushless);
+}

src/main/java/frc4388/robot/subsystems/Claw.java

@@ -1,21 +1,46 @@
 package frc4388.robot.subsystems;
+
+import java.util.Timer;
+import java.util.TimerTask;
+
+import com.revrobotics.CANSparkMax;
+
 import edu.wpi.first.wpilibj.PWM;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Claw extends SubsystemBase {
-	private PWM m_clawMotor;
-	private boolean m_open     = false;
+
+	private final PWM         m_leftMotor;
+	private final PWM         m_rightMotor;
+	private final CANSparkMax m_spinnyspin;
+
+	private boolean m_open = false;
 
     // Opens claw
-	public Claw(PWM m_clawMotor) {
-		 this.m_clawMotor = m_clawMotor;
-		 setClaw(true);
+	public Claw(PWM leftMotor, PWM rightMotor, CANSparkMax spinnyspin) {
+		m_leftMotor  = leftMotor;
+		m_rightMotor = rightMotor;
+		m_spinnyspin = spinnyspin;
+		
+		setClaw(true);
 	}
 
     public void setClaw(boolean open) {
         // Open claw
 		m_open = open;
-		m_clawMotor.setRaw(m_open ? 1000 : 2000);
+		m_leftMotor.setRaw(m_open ? 1000 : 2000);
+		m_rightMotor.setRaw(m_open ? 2000 : 1000);
+
+		if (!m_open) {
+			m_spinnyspin.set(-0.2);
+			
+			new Timer().schedule(new TimerTask() {
+				@Override
+				public void run() {
+					nospinnyspin();
+				}
+			}, 750);
+		}
     }
 
 	public void toggle() {
@@ -26,4 +51,11 @@ public class Claw extends SubsystemBase {
 		return m_open;
 	}
 
+	public void yesspinnyspin() {
+		m_spinnyspin.set(0.2);
+	}
+
+	public void nospinnyspin() {
+		m_spinnyspin.set(0);
+	}
 }