diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8cc295f..faeca52 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index f41012d..47ee185 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/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); } diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java index be04921..4899c71 100644 --- a/src/main/java/frc4388/robot/commands/Shoot.java +++ b/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; } } diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index b811a37..e9efe67 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/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);