temp work on comments and stop on current high draw

src/main/java/frc/robot/Constants.java

@@ -4,13 +4,15 @@ public final class Constants {
 	public static final class AlgaeConstants {
 		public static final int kLeftPort = 3;
 		public static final int kRightPort = 6;
-		public static final boolean kLeftInvert = false;
+		public static final boolean kLeftInvert = true;
 		public static final boolean kRightInvert = false;
 
 		public static final int kSmartCurrentLimit = 50;
 		public static final double kPeakCurrentLimit = 60;
 		public static final int kPeakCurrentDurationMillis = 100;
 		public static final double kMaxOutput = .1;
 		public static final double kMinOutput = -.1;
+
+		public static final double kCurrentToStop = .5;
 	}
 }

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

@@ -43,18 +43,20 @@ public void autonomousInit() {
 		if (m_autonomousCommand != null) {
 			m_autonomousCommand.schedule();
 		}
+		m_algaeGrabberSubsystem.setDefaultCommand(
+				m_algaeGrabberSubsystem.maxFlywheelCommand());
 	}
 
 	@Override
 	public void autonomousPeriodic() {
 		m_algaeGrabberSubsystem.setDefaultCommand(
-				m_algaeGrabberSubsystem.maxFlywheel());
+				m_algaeGrabberSubsystem.checkCurretOnFlywheelsCommand());
 	}
 
 	@Override
 	public void autonomousExit() {
 		m_algaeGrabberSubsystem.setDefaultCommand(
-				m_algaeGrabberSubsystem.stopFlywheel());
+				m_algaeGrabberSubsystem.stopFlywheelCommand());
 	}
 
 	@Override

src/main/java/frc/robot/subsystems/AlgaeGrabberSubsystem.java

@@ -16,7 +16,6 @@
 import frc.robot.Constants.AlgaeConstants;
 
 public class AlgaeGrabberSubsystem extends SubsystemBase {
-
 	private final SparkMax m_flywheelLeft;
 	private final SparkMax m_flywheelRight;
 
@@ -28,6 +27,10 @@ public AlgaeGrabberSubsystem() {
 		m_flywheelRight = new SparkMax(AlgaeConstants.kRightPort, MotorType.kBrushless);
 
 		// Initialize Motors
+		// applies the inverts and coast mode to the flywheel motors. if you want to
+		// make a motor spin the other way change it in the AlgaeConstants.k____Invert
+		// variable
+		// also applies voltage and current stuff to the motors
 		config = new SparkMaxConfig();
 		config.inverted(AlgaeConstants.kLeftInvert).idleMode(IdleMode.kCoast);
 		config.voltageCompensation(12).smartCurrentLimit(AlgaeConstants.kSmartCurrentLimit)
@@ -43,6 +46,7 @@ public AlgaeGrabberSubsystem() {
 
 	@Override
 	public void periodic() {
+		System.out.println(m_flywheelLeft.getOutputCurrent() + " | " + m_flywheelRight.getOutputCurrent());
 	}
 
 	public double getLeftVelocity() {
@@ -69,15 +73,30 @@ public void setLeftAndRightVelocity(double velocity) {
 		m_flywheelRight.set(m_setVelocity);
 	}
 
-	public Command stopFlywheel() {
+	public Command stopFlywheelCommand() {
 		return runOnce(() -> {
 			setLeftAndRightVelocity(0);
 		});
 	}
 
-	public Command maxFlywheel() {
+	public Command maxFlywheelCommand() {
 		return runOnce(() -> {
 			setLeftAndRightVelocity(.1);
 		});
 	}
+
+	public Command customFlywheelSpeedCommand(double speed) {
+		return runOnce(() -> {
+			setLeftAndRightVelocity(speed);
+		});
+	}
+
+	public Command checkCurretOnFlywheelsCommand() {
+		return runOnce(() -> {
+			if ((m_flywheelLeft.getOutputCurrent() + m_flywheelRight.getOutputCurrent())
+					/ 2 >= AlgaeConstants.kCurrentToStop) {
+				setLeftAndRightVelocity(0);
+			}
+		});
+	}
 }