Shoot command (#22)

* WIP

* Constants Tuning:
- Always boot up or deploy code to the robot in arm raised mode
- There is a negative sign on the calculate function because it technically goes from 0 to -193 (Set inverted does absolutely nothing that has been deleted)
- constant angles have been updated to reflect the offset tuning
- kDistanceSensorThreshold as 10 actually works. Needs tuning
- Intake Speed adjusted to 0.5. Needs tuning for the ground I suppose.
- kEncoderOffset adjusted

* Testing:
- Distance sensor now uses MXP port instead of onboard (added set automatic mode)
- Pid needs tuning
  -> tolerance set to 15 degrees
  - Fixed switch logic
  - Added setSetpoint at the end of setArmPosition so that the setarmpositioncommand does not automatically end
  - Have note functional
  - deleted intakemotor guards because inherent and unnecessary (add later if want)

* Modified smart dashboard to be exact

* Commit message#
t abort
qqqt
:wqq# Your branch is up to date with 'origin/shoot-command'.

* testing

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

@@ -101,23 +101,24 @@ public static final class IntakeConstants {
     public static final int kArmEncoderChannel = 0;
 
     // In degrees
-    // In degrees
-    public static final double kIntakeLoweredAngle = 0;
-    public static final double kIntakeRaisedAngle = 194;
-    public static final double kIntakeAmpScoringAngle = 100;
+    public static final double kIntakeLoweredAngle = -193;
+    public static final double kIntakeRaisedAngle = 0;
+    public static final double kIntakeAmpScoringAngle = -93; // 193 - 100 (previous angle)
 
     /** Encoder offset in rotations */
-    public static final double kArmEncoderOffset = 0.6692;
+    public static final double kArmEncoderOffset = 0.2252;
 
-    public static final double kIntakeSpeed = 3.0;
+    public static final double kIntakeSpeed = 0.5;
 
     // TODO: Tune distance sensor threshold for detecting note
     public static final double kDistanceSensorThreshold = 10;
   }
 
   public static final class ShooterConstants {
-    public static final int kTopShooterMotorPort = 35;
-    public static final int kBottomShooterMotorPort = 20;
+    public static final int kTopShooterMotorPort = 20;
+    public static final int kBottomShooterMotorPort = 35;
+    public static final double kShooterSpeed = 0.7;
+    public static final double kShooterOff = 0;
   }    
   public static class ClimberConstants {
         public final static int leftForwardChannel = 0;

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

@@ -23,7 +23,7 @@ public class Robot extends TimedRobot {
 
   private RobotContainer m_robotContainer;
 
-  public ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
+  // public ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
 
   private Timer m_buttonTimer = new Timer();
   /**
@@ -59,10 +59,10 @@ public void disabledInit() {}
 
   @Override
   public void disabledPeriodic() {
-    if (RobotController.getUserButton() && m_buttonTimer.get() > 1) {
-      m_climberSubsystem.toggleCompressor();
-      m_buttonTimer.reset();
-    }
+    // if (RobotController.getUserButton() && m_buttonTimer.get() > 1) {
+    //   m_climberSubsystem.toggleCompressor();
+    //   m_buttonTimer.reset();
+    // }
   }
 
   /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */

src/main/java/frc/robot/RobotContainer.java

@@ -19,6 +19,7 @@
 import edu.wpi.first.wpilibj.XboxController.Button;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -28,10 +29,12 @@
 import frc.robot.commands.IntakeArmPositionCommand;
 import frc.robot.commands.NoteIntakeCommand;
 import frc.robot.commands.NoteOuttakeCommand;
+import frc.robot.commands.ShooterSetSpeedCommand;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.IntakeSubsystem.ArmPosition;
+import frc.robot.subsystems.ShooterSubsystem.ShootSpeed;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.VisionSubsystem;
 
@@ -46,7 +49,7 @@ public class RobotContainer {
   private final DriveSubsystem m_robotDrive = new DriveSubsystem();
   private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
   private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
-  private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
+  // private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
   private final VisionSubsystem m_visionSubsystem = new VisionSubsystem();
 
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
@@ -118,17 +121,14 @@ private void configureBindings() {
     // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5,
     // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5)));
 
-    new JoystickButton(m_operatorController, Button.kX.value)
-        .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem))
-        .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
-    new JoystickButton(m_operatorController, Button.kY.value)
-        .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem))
-        .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
-
-    new JoystickButton(m_operatorController, Button.kA.value)
-        .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_climberSubsystem));
-    new JoystickButton(m_operatorController, Button.kB.value)
-        .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_climberSubsystem));
+    new JoystickButton(m_driverController, Button.kX.value)
+        .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting), new NoteOuttakeCommand(m_intakeSubsystem)))
+        .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off));
+
+    // new JoystickButton(m_operatorController, Button.kA.value)
+    //     .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_climberSubsystem));
+    // new JoystickButton(m_operatorController, Button.kB.value)
+    //     .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_climberSubsystem));
 
     new Trigger(() -> {
       return m_driverController.getLeftTriggerAxis() > 0.5;

src/main/java/frc/robot/commands/NoteIntakeCommand.java

@@ -5,6 +5,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.IntakeConstants;
 import frc.robot.subsystems.IntakeSubsystem;
 
 public class NoteIntakeCommand extends Command {
@@ -39,6 +40,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return m_intakeSubsystem.haveNote();
+    return m_intakeSubsystem.haveNote() || m_intakeSubsystem.getArmPosition() != IntakeConstants.kIntakeLoweredAngle;
   }
 }

src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.ShooterConstants;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.ShooterSubsystem.ShootSpeed;
+
+public class ShooterSetSpeedCommand extends Command {
+  ShooterSubsystem m_ShooterSubsystem;
+  double m_shooterSpeed = 0;
+
+  Timer m_timer = new Timer();
+
+  ShootSpeed m_shootSpeed;
+
+  /** Creates a new ShootCommand. */
+  public ShooterSetSpeedCommand(ShooterSubsystem shooterSubsystem, ShootSpeed shootSpeed) {
+    m_ShooterSubsystem = shooterSubsystem;
+    addRequirements(m_ShooterSubsystem);
+
+    m_shootSpeed = shootSpeed;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_ShooterSubsystem.setShootingSpeed(m_shootSpeed);
+
+    m_timer.reset();
+    m_timer.start();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_timer.get() > 1;
+  }
+}

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

@@ -23,7 +23,7 @@ public class IntakeSubsystem extends SubsystemBase {
   private CANSparkFlex m_intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless);
   private CANSparkFlex m_armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless);
 
-  private PIDController m_armPID = new PIDController(0.0015, 0, 0);
+  private PIDController m_armPID = new PIDController(0.002, 0, 0);
 
   private DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel);
 
@@ -34,17 +34,18 @@ public class IntakeSubsystem extends SubsystemBase {
 
   /** Creates a new IntakeSubsystem */
   public IntakeSubsystem() {
-    // m_armMotor.setInverted(IntakeConstants.kArmMotorInverted);
-    // m_intakeMotor.setInverted(IntakeConstants.kIntakeMotorInverted);
+    m_distanceSensor.setAutomaticMode(true);
 
     m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset);
+    SmartDashboard.putNumber("arm", m_armEncoder.getAbsolutePosition());
     m_armEncoder.setDistancePerRotation(360);
 
-    m_intakeMotor.setIdleMode(IdleMode.kCoast);
+    m_intakeMotor.setIdleMode(IdleMode.kBrake);
     m_armMotor.setIdleMode(IdleMode.kCoast);
 
-    // m_armPID.disableContinuousInput();
-    m_armPID.setTolerance(0.05);
+    m_armPID.setTolerance(10);
+
+    m_armSetpoint = m_armEncoder.getDistance();
   }
 
   public void setArmPosition(ArmPosition position) {
@@ -53,13 +54,19 @@ public void setArmPosition(ArmPosition position) {
         m_armSetpoint = IntakeConstants.kIntakeAmpScoringAngle;
         break;
       case Extended:
-        m_armSetpoint = IntakeConstants.kIntakeRaisedAngle;
+        m_armSetpoint = IntakeConstants.kIntakeLoweredAngle;
         break;
       case Retracted:
-        m_armSetpoint = IntakeConstants.kIntakeLoweredAngle;
+        m_armSetpoint = IntakeConstants.kIntakeRaisedAngle;
       default:
         break;
     }
+
+    m_armPID.setSetpoint(m_armSetpoint);
+  }
+
+  public double getArmPosition(){
+    return m_armSetpoint;
   }
 
   public boolean armAtSetpoint() {
@@ -71,7 +78,7 @@ public void intake() {
   }
 
   public void outtake() {
-    m_intakeSpeed = -IntakeConstants.kIntakeSpeed;
+    m_intakeSpeed = -IntakeConstants.kIntakeSpeed - 0.5;
   }
 
   public void stopIntake() {
@@ -89,11 +96,16 @@ public double getDistanceSensor() {
   public void periodic() {
     haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold;
 
-    // m_armMotor.set(MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3));
-    // m_intakeMotor.set((m_intakeSpeed >= 0 && haveNote) ? 0 : m_intakeSpeed);
+    //Note: negative because encoder goes from 0 to -193 cuz weird
+    double setMotorSpeed = MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.4, 0.4);
+    m_armMotor.set(setMotorSpeed);
+    m_intakeMotor.set(m_intakeSpeed);
+    SmartDashboard.putNumber("intakespeed", m_intakeSpeed);
 
     SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance());
     SmartDashboard.putBoolean("Have Note?", haveNote);
+    SmartDashboard.putNumber("distance sensor", m_distanceSensor.getRange(Rev2mDistanceSensor.Unit.kInches));
+    SmartDashboard.putNumber("pid output", setMotorSpeed);
   }
 
   public boolean haveNote() {

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

@@ -5,8 +5,10 @@
 package frc.robot.subsystems;
 
 import com.revrobotics.CANSparkFlex;
+import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.ShooterConstants;
 
@@ -15,18 +17,48 @@ public class ShooterSubsystem extends SubsystemBase {
   CANSparkFlex m_bottom = new CANSparkFlex(ShooterConstants.kBottomShooterMotorPort, MotorType.kBrushless);
   CANSparkFlex m_top = new CANSparkFlex(ShooterConstants.kTopShooterMotorPort, MotorType.kBrushless);
 
+  private double m_topSpeed = 0;
+  private double m_bottomSpeed = 0;
+
   public ShooterSubsystem() {
+    m_bottom.setIdleMode(IdleMode.kCoast);
+    m_top.setIdleMode(IdleMode.kCoast);
+
+    m_bottom.setInverted(true);
+    m_top.setInverted(true);
+  }
 
+  public void setShootingSpeed(ShootSpeed speed) {
+    switch (speed){
+      case Shooting:
+        m_topSpeed = ShooterConstants.kShooterSpeed;
+        m_bottomSpeed = ShooterConstants.kShooterSpeed;
+        // System.out.println("shoot speed: " + ShooterConstants.kShooterSpeed);
+        break;
+      case Off:
+        m_topSpeed = 0.0;
+        m_bottomSpeed = 0.0;
+        // System.out.println("shoot speed: " + 0);
+        break;
+    }
   }
 
-  public void spin(double speed) {
-    m_bottom.set(speed);
-    m_top.set(speed);
+  public double returnCurrentSpeed(){
+    return m_bottom.getEncoder().getVelocity();
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    // SmartDashboard.putNumber("Speed", m_bottom.);
+    SmartDashboard.putNumber("bottom Speed", m_bottomSpeed);
+    SmartDashboard.putNumber("top Speed", m_topSpeed);
+
+    m_bottom.set(m_bottomSpeed);
+    m_top.set(m_topSpeed);
   }
+
+  public static enum ShootSpeed{
+    Shooting,
+    Off
+  } 
 }