implenmented NEO motor on intake

Author: aaron345678

src/main/java/org/carlmontrobotics/RobotContainer.java

@@ -137,7 +137,8 @@ public RobotContainer() {
 
     setDefaultCommands();
     setBindingsDriver();
-    setBindingsManipulatorENDEFF();
+    //setBindingsManipulatorENDEFF();
+    setBindingsManipulatorNEO();
   }
 
   private void setDefaultCommands() {
@@ -180,7 +181,36 @@ private void setBindingsDriver() {
     new JoystickButton(driverController, Driver.rotateFieldRelative270Deg)
         .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(90), drivetrain));
   }
+  private void setBindingsManipulatorNEO() {
+    new JoystickButton(manipulatorController, EJECT_BUTTON).onTrue(new Eject(intakeShooter));
+
 
+    new JoystickButton(manipulatorController, Button.kB.value).onTrue(new RampMaxRPM(intakeShooter));
+    new JoystickButton(manipulatorController, Button.kB.value).onFalse(new InstantCommand(intakeShooter::stopOutake,intakeShooter));
+   new JoystickButton(manipulatorController, AMP_BUTTON).onTrue(new EjectOuttakeSide(intakeShooter));
+
+    axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON)
+        .onTrue(
+            new SwitchRPMShootNEO(intakeShooter));
+    axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON)
+        .onFalse(
+            new InstantCommand(intakeShooter::stopOutake, intakeShooter));
+
+    axisTrigger(manipulatorController, Manipulator.INTAKE_BUTTON)
+        .onTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"),
+            new IntakeNEO(intakeShooter)));
+    axisTrigger(manipulatorController, Manipulator.INTAKE_BUTTON)
+        .onFalse(
+            new InstantCommand(intakeShooter::stopIntake, intakeShooter));
+    new JoystickButton(manipulatorController, Button.kY.value).onTrue(new ArmToPos(arm, AMP_ANGLE_RAD_NEW_MOTOR,0));
+    new JoystickButton(manipulatorController, Button.kA.value).onTrue(new ArmToPos(arm, GROUND_INTAKE_POS,1));
+    new JoystickButton(manipulatorController, Button.kLeftStick.value).onTrue(new PassToOuttake(intakeShooter));
+    new JoystickButton(manipulatorController, Button.kX.value).onTrue(new ArmToPos(arm, SPEAKER_ANGLE_RAD,1));
+    //TODO: test angles for pov button BEFORE climbing
+    new POVButton(manipulatorController, 0).onTrue(new ArmToPos(arm, CLIMB_POS, 0));
+    new POVButton(manipulatorController, 180).onTrue(new Climb(arm));
+    new POVButton(manipulatorController, 270).onTrue(new ArmToPos(arm, PODIUM_ANGLE_RAD, 2));
+  }
   private void setBindingsManipulatorENDEFF() {
     /* /Eject also for AMP/ */
     new JoystickButton(manipulatorController, EJECT_BUTTON).onTrue(new Eject(intakeShooter));

src/main/java/org/carlmontrobotics/commands/Intake.java

@@ -25,8 +25,6 @@ public Intake(IntakeShooter intake) {
   @Override
   public void initialize() {
     intake.setRPMIntake(INTAKE_RPM);
-    timer.reset();
-    timer.start();
     intake.resetCurrentLimit();
     index=0; 
 
@@ -36,7 +34,7 @@ public void initialize() {
   @Override
   public void execute() {
     // Intake Led
-    intake.resetCurrentLimit();
+    
     if (intake.intakeDetectsNote() && !intake.outakeDetectsNote()) {
       index++;
 
@@ -51,6 +49,7 @@ public void execute() {
     if(!intake.intakeDetectsNote()) {
       intake.setRPMIntake(INTAKE_RPM);
     }
+    
   }
 
   // Called once the command ends or is interrupted.

src/main/java/org/carlmontrobotics/commands/IntakeNEO.java

@@ -0,0 +1,59 @@
+package org.carlmontrobotics.commands;
+
+import static org.carlmontrobotics.Constants.Effectorc.*;
+
+import org.carlmontrobotics.subsystems.IntakeShooter;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class IntakeNEO extends Command {
+  // intake until sees game peice or 4sec has passed
+  private Timer timer = new Timer();
+  private final IntakeShooter intake;
+
+  private double endAt = 0;
+  private final double keepIntakingFor = 0.2;
+  int increaseAmount = 750;
+  int index = 0;
+
+  public IntakeNEO(IntakeShooter intake) {
+    addRequirements(this.intake = intake);
+
+  }
+
+  @Override
+  public void initialize() {
+    //TODO: Adjust speed or add in an index
+    intake.motorSetIntake(0.5);
+    intake.resetCurrentLimit();
+    index=0; 
+    
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    // Intake Led
+    
+   
+    
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    intake.stopIntake();
+    timer.stop();
+    index = 0;
+    //intake.resetCurrentLimit();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return (intake.intakeDetectsNote() && intake.outakeDetectsNote());
+    // || //timer.hasElapsed(MAX_SECONDS_OVERLOAD);
+
+  }
+}

src/main/java/org/carlmontrobotics/commands/SwitchRPMShootNEO.java

@@ -0,0 +1,46 @@
+package org.carlmontrobotics.commands;
+
+import static org.carlmontrobotics.Constants.Armc.SMART_CURRENT_LIMIT_TIMEOUT;
+import static org.carlmontrobotics.Constants.Effectorc.*;
+import org.carlmontrobotics.subsystems.Arm;
+import org.carlmontrobotics.subsystems.IntakeShooter;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class SwitchRPMShootNEO extends Command {
+    private IntakeShooter intakeShooter;
+    private double rpmAmount;
+   
+    
+    public SwitchRPMShootNEO(IntakeShooter intakeShooter) {
+        this.intakeShooter = intakeShooter;
+        addRequirements(intakeShooter);
+    }
+    @Override
+    public void initialize() {
+        intakeShooter.setMaxOutake(1);
+        
+    }
+    @Override
+    public void execute() {
+        rpmAmount = RPM_SELECTOR[Arm.getSelector()];
+        if(intakeShooter.getOutakeRPM() >= rpmAmount) {
+        intakeShooter.setMaxIntake(1);
+       
+        }
+    }
+    @Override
+    public void end(boolean interrupted) {
+        intakeShooter.stopIntake();
+        intakeShooter.stopOutake();
+        intakeShooter.resetCurrentLimit();
+      
+        
+    }
+    @Override
+    public boolean isFinished() {
+        return (!intakeShooter.intakeDetectsNote() && !intakeShooter.outakeDetectsNote());
+    }
+}

src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java

@@ -21,7 +21,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class IntakeShooter extends SubsystemBase {
-    private final CANSparkMax intakeMotor = MotorControllerFactory.createSparkMax(INTAKE_PORT, MotorConfig.NEO_550);
+    private final CANSparkMax intakeMotor = MotorControllerFactory.createSparkMax(INTAKE_PORT, MotorConfig.NEO);
     // private final CANSparkMax outakeMotor =
     // MotorControllerFactory.createSparkMax(10, MotorConfig.NEO_550);
     private final CANSparkFlex outakeMotorVortex = new CANSparkFlex(10, MotorType.kBrushless);
@@ -85,7 +85,9 @@ private double getGamePieceDistanceIntake() {
     public void motorSetOutake(int speed) {
         outakeMotorVortex.set(speed);
     }
-
+    public void motorSetIntake(double speed) {
+        intakeMotor.set(speed);
+    }
     private double getGamePieceDistanceOutake() {
         return Units.metersToInches(OutakeDistanceSensor.getRange() / 1000) - DS_DEPTH_INCHES;
     }
@@ -139,7 +141,7 @@ public void setMaxOutakeOverload(int direction) {
         outakeMotorVortex.set(1 * direction);
     }
     public void resetCurrentLimit() {
-        intakeMotor.setSmartCurrentLimit(MotorConfig.NEO_550.currentLimitAmps);
+        intakeMotor.setSmartCurrentLimit(MotorConfig.NEO.currentLimitAmps);
         outakeMotorVortex.setSmartCurrentLimit(60);
 
         // intakeMotor.setSmartCurrentLimit(MotorConfig.NEO_550.currentLimitAmps);