mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Implement field positions, tolerance, LED states
This commit is contained in:
Michael Mikovsky
@@ -1,7 +1,5 @@
|
||||
package frc4388.robot.subsystems.climber;
|
||||
|
||||
import static edu.wpi.first.units.Units.Rotation;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
@@ -9,20 +7,19 @@ import org.littletonrobotics.junction.Logger;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
|
||||
public class Climber extends SubsystemBase {
|
||||
ClimberIO io;
|
||||
ClimberStateAutoLogged state = new ClimberStateAutoLogged();
|
||||
|
||||
Supplier<Pose2d> m_swervePoseSupplier;
|
||||
// Supplier<Pose2d> m_swervePoseSupplier;
|
||||
|
||||
public Climber(
|
||||
ClimberIO io,
|
||||
Supplier<Pose2d> swervePoseSupplier
|
||||
ClimberIO io
|
||||
// Supplier<Pose2d> swervePoseSupplier
|
||||
) {
|
||||
this.io = io;
|
||||
this.m_swervePoseSupplier = swervePoseSupplier;
|
||||
// this.m_swervePoseSupplier = swervePoseSupplier;
|
||||
}
|
||||
|
||||
// public enum ClimberMode {
|
||||
@@ -44,19 +41,6 @@ public class Climber extends SubsystemBase {
|
||||
// }
|
||||
|
||||
|
||||
// public enum FieldZone {
|
||||
// // The robot should aim at the hub
|
||||
// InShootZone,
|
||||
// // The robot should aim towards the wall
|
||||
// AimAtWall,
|
||||
|
||||
// }
|
||||
|
||||
// // Calculate what should be done based off of the position of the robot
|
||||
// // TODO: Implement field zones
|
||||
// public FieldZone getTarget(Pose2d position) {
|
||||
// return FieldZone.InShootZone;
|
||||
// }
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
@@ -68,9 +52,6 @@ public class Climber extends SubsystemBase {
|
||||
|
||||
Logger.processInputs("Climber", state);
|
||||
|
||||
Pose2d pose = m_swervePoseSupplier.get();
|
||||
Angle robotRot = pose.getRotation().getMeasure();
|
||||
|
||||
io.updateInputs(state);
|
||||
|
||||
}
|
||||
|
||||
@@ -2,15 +2,21 @@ package frc4388.robot.subsystems.shooter;
|
||||
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||
|
||||
import java.text.FieldPosition;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLogOutput;
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.intake.Intake;
|
||||
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.utility.compute.FieldPositions;
|
||||
|
||||
public class Shooter extends SubsystemBase {
|
||||
public ShooterIO io;
|
||||
@@ -57,10 +63,6 @@ public class Shooter extends SubsystemBase {
|
||||
|
||||
private ShooterMode mode = ShooterMode.NotReady;
|
||||
|
||||
// public void setMode(ShooterMode mode) {
|
||||
// this.mode = mode;
|
||||
// }
|
||||
|
||||
public void setShooterReady() {
|
||||
if(this.mode == ShooterMode.NotReady) {
|
||||
this.mode = ShooterMode.Ready;
|
||||
@@ -86,14 +88,65 @@ public class Shooter extends SubsystemBase {
|
||||
io.updateInputs(state);
|
||||
|
||||
if(this.mode != ShooterMode.NotReady) {
|
||||
double badRobotSpeed = Math.sqrt(Math.pow(m_SwerveDrive.chassisSpeeds.vxMetersPerSecond,2) + Math.pow(m_SwerveDrive.chassisSpeeds.vyMetersPerSecond,2));
|
||||
double badAngSpeed = Math.abs(m_SwerveDrive.chassisSpeeds.omegaRadiansPerSecond);
|
||||
double badShooterVelocity = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
|
||||
|
||||
ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds;
|
||||
double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2));
|
||||
double AngSpeed = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI));
|
||||
|
||||
Pose2d robotPose2d = m_SwerveDrive.getPose2d();
|
||||
|
||||
double distanceToHub = robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm();
|
||||
|
||||
|
||||
// TODO: get if the robot is within the angle of the hub
|
||||
|
||||
boolean driverError =
|
||||
XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() |
|
||||
AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() |
|
||||
distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() |
|
||||
distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get();
|
||||
|
||||
double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
|
||||
|
||||
boolean badShooterVelocity = shooterSpeed < ShooterConstants.SHOOTER_SPEED_TOLERANCE.get();
|
||||
boolean intakeBad = m_Intake.getMode() == IntakeMode.Extended;
|
||||
|
||||
|
||||
int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + (intakeBad ? 4 : 0);
|
||||
switch (bitmask) {
|
||||
case 0b000: // No Errors
|
||||
m_robotLED.setMode(Constants.LEDConstants.OPREADY);
|
||||
break;
|
||||
case 0b001: // No op err, yes driver err
|
||||
m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS);
|
||||
break;
|
||||
case 0b010: // Bad flywheel, no driver err
|
||||
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
|
||||
break;
|
||||
case 0b011: // Bad flywheel, yes driver err
|
||||
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
|
||||
break;
|
||||
case 0b100: // Bad intake, no driver err
|
||||
m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT);
|
||||
break;
|
||||
case 0b101: // Bad intake, yes driver err
|
||||
m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS);
|
||||
break;
|
||||
case 0b110: // Bad intake and shooter (intake is more important), no driver err
|
||||
m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT);
|
||||
break;
|
||||
case 0b111: // Bad intake and shooter (intake is more important), yes driver err
|
||||
m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS);
|
||||
break;
|
||||
}
|
||||
|
||||
// We set the shooter mode to ready if there are no errors
|
||||
mode = (
|
||||
bitmask == 0 ?
|
||||
ShooterMode.Ready :
|
||||
ShooterMode.NotReady
|
||||
);
|
||||
|
||||
// TODO: get if the robot is within the correct distance of the hub
|
||||
}
|
||||
|
||||
switch (mode) {
|
||||
|
||||
@@ -31,21 +31,21 @@ public class ShooterConstants {
|
||||
|
||||
public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 30);
|
||||
public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15);
|
||||
// public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0);
|
||||
|
||||
public static final ConfigurableDouble INDEXER_FORWARD_VELOCITY = new ConfigurableDouble("Indexer FWD Velocity", 10);
|
||||
public static final ConfigurableDouble INDEXER_REVERSE_VELOCITY = new ConfigurableDouble("Indexer reverse Velocity", 10);
|
||||
|
||||
|
||||
// Tolerances
|
||||
public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist", 1);
|
||||
public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist", 1);
|
||||
public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 0);
|
||||
public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 99);
|
||||
|
||||
public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Shoot Ang tolerance", 1);
|
||||
public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360);
|
||||
|
||||
public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Shoot speed tolerance", 1);
|
||||
public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance", 1);
|
||||
public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1);
|
||||
public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance DEG", 3);
|
||||
|
||||
public static final ConfigurableDouble SHOOT_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance", 1);
|
||||
public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 1);
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user