Merge branch 'vision-odo-not-proto' of https://github.com/Team4388/2022NoWayHome into vision-odo-not-proto

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

@@ -96,11 +96,11 @@ public class RobotContainer {
   private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter);
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
   private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
-  private final Hood m_robotHood = new Hood();
+  private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
   private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
   private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
   private final Vision m_robotVision = new Vision(m_robotTurret, m_robotBoomBoom);
-
+  
   /* Controllers */
   private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
   private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);

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

@@ -13,6 +13,7 @@ import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.DigitalInput;
@@ -32,17 +33,15 @@ import frc4388.robot.subsystems.SwerveModule;
 public class RobotMap {
 
   public RobotMap() {
-    // configureLEDMotorControllers();
+    configureLEDMotorControllers();
     configureSwerveMotorControllers();
-    // configureShooterMotorControllers();
+    configureShooterMotorControllers();
   }
 
   /* LED Subsystem */
   public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
 
-  void configureLEDMotorControllers() {
-
-  }
+  void configureLEDMotorControllers() {}
 
   /* Swerve Subsystem */
 
@@ -167,8 +166,12 @@ public class RobotMap {
   public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
   public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
 
+  // turret subsystem
   public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
 
+  // hood subsystem
+  public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
+
   // Create motor CANSparkMax
   void configureShooterMotorControllers() {
 
@@ -209,6 +212,9 @@ public class RobotMap {
     // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull
     // numbers out of our ass anymore
 
+    // hood subsystem
+    angleAdjusterMotor.restoreFactoryDefaults();
+    angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
   }
 
 

src/main/java/frc4388/robot/commands/Shoot.java

@@ -7,6 +7,7 @@ package frc4388.robot.commands;
 import edu.wpi.first.hal.simulation.SimulatorJNI;
 import edu.wpi.first.wpilibj.simulation.LinearSystemSim;
 import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.subsystems.BoomBoom;
@@ -44,7 +45,8 @@ public class Shoot extends CommandBase {
   public double proportional, integral, derivative;
   public double time;
   public double output;
-  public double tolerance = 5.0;
+  public double tolerance;
+  public boolean isAimedInTolerance;
 
   // testing
   public DummySensor dummy = new DummySensor(0);
@@ -75,6 +77,9 @@ public class Shoot extends CommandBase {
     derivative = 0;
     time = 0.02;
 
+    tolerance = 5.0;
+    isAimedInTolerance = false;
+
     DummySensor.resetAll();
   }
 
@@ -83,6 +88,8 @@ public class Shoot extends CommandBase {
    */
   public void updateError() {
     error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
+    isAimedInTolerance = (Math.abs(error) <= tolerance);
+    SmartDashboard.putBoolean("isAimed?", isAimedInTolerance);
   }
 
   // Called when the command is initially scheduled.
@@ -154,7 +161,6 @@ public class Shoot extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    updateError();
-    return Math.abs(error) <= tolerance;
+    return false;
   }
 }

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

@@ -24,7 +24,7 @@ import frc4388.utility.Gains;
 public class Hood extends SubsystemBase {
   public BoomBoom m_shooterSubsystem;
   
-  public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
+  public CANSparkMax m_angleAdjusterMotor;
   public SparkMaxLimitSwitch m_hoodUpLimitSwitch;
   public SparkMaxLimitSwitch m_hoodDownLimitSwitch;
   public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
@@ -39,8 +39,9 @@ public double m_fireAngle;
   
 
   /** Creates a new Hood. */
-  public Hood() {
-     m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
+  public Hood(CANSparkMax angleAdjusterMotor) {
+
+    m_angleAdjusterMotor = angleAdjusterMotor;
 
     m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
     m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);