testing

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

@@ -115,8 +115,8 @@ public static final class IntakeConstants {
   }
 
   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;
   }    

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

@@ -121,9 +121,9 @@ private void configureBindings() {
     // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5,
     // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5)));
 
-    new JoystickButton(m_operatorController, Button.kX.value)
-        .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, true), new NoteOuttakeCommand(m_intakeSubsystem)))
-        .onFalse(new InstantCommand(() -> m_shooterSubsystem.setShootingSpeed(ShootSpeed.Off), m_shooterSubsystem));
+    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));

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

@@ -16,25 +16,20 @@ public class ShooterSetSpeedCommand extends Command {
 
   Timer m_timer = new Timer();
 
-  boolean bool = false;
+  ShootSpeed m_shootSpeed;
 
   /** Creates a new ShootCommand. */
-  public ShooterSetSpeedCommand(ShooterSubsystem shooterSubsystem, boolean shoot) {
+  public ShooterSetSpeedCommand(ShooterSubsystem shooterSubsystem, ShootSpeed shootSpeed) {
     m_ShooterSubsystem = shooterSubsystem;
     addRequirements(m_ShooterSubsystem);
 
-    bool = shoot;
+    m_shootSpeed = shootSpeed;
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    if (bool){
-      m_ShooterSubsystem.setShootingSpeed(ShootSpeed.Shooting);
-    }
-    else{
-      m_ShooterSubsystem.setShootingSpeed(ShootSpeed.Off);
-    }
+    m_ShooterSubsystem.setShootingSpeed(m_shootSpeed);
 
     m_timer.reset();
     m_timer.start();
@@ -51,6 +46,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return m_timer.get() > 0.5;
+    return m_timer.get() > 1;
   }
 }

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

@@ -23,11 +23,11 @@ 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.0014, 0, 0);
+  private PIDController m_armPID = new PIDController(0.002, 0, 0);
 
   private DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel);
 
-  private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kMXP); // onboard I2C port;
+  private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kOnboard); // onboard I2C port;
 
   private double m_intakeSpeed = 0;
   private double m_armSetpoint = IntakeConstants.kIntakeRaisedAngle;
@@ -43,7 +43,7 @@ public IntakeSubsystem() {
     m_intakeMotor.setIdleMode(IdleMode.kBrake);
     m_armMotor.setIdleMode(IdleMode.kCoast);
 
-    m_armPID.setTolerance(15);
+    m_armPID.setTolerance(10);
 
     m_armSetpoint = m_armEncoder.getDistance();
   }
@@ -78,7 +78,7 @@ public void intake() {
   }
 
   public void outtake() {
-    m_intakeSpeed = -IntakeConstants.kIntakeSpeed;
+    m_intakeSpeed = -IntakeConstants.kIntakeSpeed - 0.5;
   }
 
   public void stopIntake() {
@@ -97,7 +97,7 @@ public void periodic() {
     haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold;
 
     //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);
+    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);

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

@@ -8,6 +8,7 @@
 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;
 
@@ -23,23 +24,35 @@ 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 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);
   }