diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 47e32e61..8258478d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -391,6 +391,7 @@ public class CollectorConstants { public static final double SIM_KI = 0.05; public static final double SIM_KD = 0; public static final double SIM_KA = 0.145; + public static final double MaxAngularRate = 10; // m/s (this is a total geuss for simulatiuon) public static final double COLLECTOR_SYSTEST_POWER = 0.25; public static final double COLLECTOR_GRABANDGO_POWER = 0.75; @@ -466,6 +467,7 @@ public enum PieceState { public static final double SIM_KP = 0.1; public static final double SIM_KI = 0.05; public static final double SIM_KD = 0; + public static final double MaxAngularRate = 10; // m/s (this is a total geuss for simulatiuon) } public class MercuryPivotConstants { @@ -793,6 +795,9 @@ public class PeiceSimConstants { public static final double COLLECT_DISTANCE = 1; public static final Rotation2d COLLECT_ANGLE_MAX = new Rotation2d(1d); public static final Rotation2d COLLECT_ANGLE_MIN = new Rotation2d(-1d); + + public static final double DistanceFromStartToIndexerEntry = 0.0762; // meters + public static final double DistanceFromStartToIndexerExit = 0.33; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 193fd5c9..766e56e2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -381,11 +381,11 @@ protected void configureButtonBindings() { .setDrivetrainPose(StartingPoseConstants.SOURCE_SUB_C_STARTPOSE_RED))); // TESTSIMS - new Trigger(() -> RobotBase.isSimulation() && DriverStation.isEnabled()).whileTrue( // TODO: remove - new Index(() -> 0.6, indexer)); + // new Trigger(() -> RobotBase.isSimulation() && DriverStation.isEnabled()).whileTrue( // TODO: remove + // new NotePass(drivetrain, flywheel, pivot, driver, indexer).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); - new Trigger(() -> RobotBase.isSimulation() && !DriverStation.isEnabled()).whileTrue( // TODO: remove - new Index(() -> 0, indexer)); + // new Trigger(() -> RobotBase.isSimulation() && !DriverStation.isEnabled()).whileTrue( // TODO: remove + // new Index(() -> 0, indexer)); } @Override diff --git a/src/main/java/frc/robot/command/shoot/NotePass.java b/src/main/java/frc/robot/command/shoot/NotePass.java index 1c8c84c5..04ea5907 100644 --- a/src/main/java/frc/robot/command/shoot/NotePass.java +++ b/src/main/java/frc/robot/command/shoot/NotePass.java @@ -109,9 +109,11 @@ public void execute() { pivot.setTargetAngle(ShooterConstants.NOTEPASS_ANGLE_MAP.get(distanceToCorner) + pivot.getBias()); if (flywheel.allMotorsOnTarget() && pivot.onTarget() && inTolerance()) { - if (DriverStation.isAutonomous()) { - indexer.indexUp(); - } + // if (DriverStation.isAutonomous()) { + // indexer.indexUp(); + // } + indexer.indexUp(); + System.out.println("passing note"); new TimedCommand(RobotContainer.hapticCopilotCommand(), 1d).schedule(); new TimedCommand(RobotContainer.hapticDriverCommand(), 1d).schedule(); } @@ -159,10 +161,11 @@ private void initLogging() { shooterOnTargetLog = new BooleanLogEntry(log, "/NotePass/Shooter-OnTarget"); if (!DriverStation.isFMSAttached()) { - LightningShuffleboard.setBoolSupplier("Note-Pass", "In tloeracnce", () -> inTolerance()); + LightningShuffleboard.setBoolSupplier("Note-Pass", "In toleracnce", () -> inTolerance()); LightningShuffleboard.setDoubleSupplier("Note-Pass", "CurrentHeading", () -> currentHeading); LightningShuffleboard.setDoubleSupplier("Note-Pass", "TargetHeading", () -> targetHeading); LightningShuffleboard.setDoubleSupplier("Note-Pass", "Distance to Corner", () -> distanceToCorner); + LightningShuffleboard.setDoubleSupplier("Note-Pass", "PID Output", () -> pidOutput); } } diff --git a/src/main/java/frc/robot/command/shoot/SmartShoot.java b/src/main/java/frc/robot/command/shoot/SmartShoot.java index 91ebcc36..1445eeac 100644 --- a/src/main/java/frc/robot/command/shoot/SmartShoot.java +++ b/src/main/java/frc/robot/command/shoot/SmartShoot.java @@ -96,11 +96,13 @@ public void execute() { state = ShootingState.SHOOT; shotTime = Timer.getFPGATimestamp(); } + System.out.println("Aiming :)"); break; case SHOOT: // Continues aiming, indexs to shoot pivot.setTargetAngle(calculateTargetAngle(distance)); flywheel.setAllMotorsRPM(calculateTargetRPM(distance)); + System.out.println("Indexing :)"); indexer.indexUp(); collector.setPower(.75d); // Once shoot critera met moves to shot diff --git a/src/main/java/frc/robot/subsystems/Collector.java b/src/main/java/frc/robot/subsystems/Collector.java index 39ed1109..c020a577 100644 --- a/src/main/java/frc/robot/subsystems/Collector.java +++ b/src/main/java/frc/robot/subsystems/Collector.java @@ -19,6 +19,7 @@ import frc.robot.Constants.RobotMap.DIO; import frc.robot.Constants; import frc.robot.Constants.CollectorConstants; +import frc.thunder.hardware.LightningBeamBreak; import frc.thunder.hardware.ThunderBird; import frc.thunder.shuffleboard.LightningShuffleboard; @@ -34,7 +35,7 @@ public class Collector extends SubsystemBase { // Declare collector hardware private ThunderBird motor; - private DigitalInput beamBreak; + public LightningBeamBreak beamBreak; private final VelocityVoltage velocityVoltage = new VelocityVoltage( 0, 0, true, CollectorConstants.MOTOR_KV, @@ -59,7 +60,7 @@ public Collector() { motor.configPIDF(0, CollectorConstants.MOTOR_KP, CollectorConstants.MOTOR_KI, CollectorConstants.MOTOR_KD, CollectorConstants.MOTOR_KS, CollectorConstants.MOTOR_KV); - beamBreak = new DigitalInput(DIO.COLLECTOR_BEAMBREAK); + beamBreak = new LightningBeamBreak(DIO.COLLECTOR_BEAMBREAK); motor.applyConfig(); initLogging(); diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 09a1acec..ad37b19d 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -22,6 +22,7 @@ import frc.robot.Constants.IndexerConstants.PieceState; import frc.robot.Constants.RobotMap.CAN; import frc.robot.Constants.RobotMap.DIO; +import frc.thunder.hardware.LightningBeamBreak; import frc.thunder.hardware.ThunderBird; import frc.thunder.shuffleboard.LightningShuffleboard; @@ -37,8 +38,8 @@ public class Indexer extends SubsystemBase { private Collector collector; private ThunderBird motor; - private DigitalInput indexerSensorEntry = new DigitalInput(DIO.INDEXER_ENTER_BEAMBREAK); - private DigitalInput indexerSensorExit = new DigitalInput(DIO.INDEXER_EXIT_BEAMBREAK); + public LightningBeamBreak indexerSensorEntry = new LightningBeamBreak(DIO.INDEXER_ENTER_BEAMBREAK); + public LightningBeamBreak indexerSensorExit = new LightningBeamBreak(DIO.INDEXER_EXIT_BEAMBREAK); private DutyCycleOut dutyCycleControl = new DutyCycleOut(0).withEnableFOC(true); @@ -234,6 +235,8 @@ public void simulationPeriodic() { // set inputs for simulation double pidoutput = indexerController.calculate(indexerSim.getOutput(0), targetPower); + LightningShuffleboard.setDouble("Indexer", "pidoutput", pidoutput); + indexerSim.setInput(pidoutput * 12); indexerSim.update(0.01); } diff --git a/src/main/java/frc/robot/subsystems/PivotRhapsody.java b/src/main/java/frc/robot/subsystems/PivotRhapsody.java index df3c86a6..df922af1 100644 --- a/src/main/java/frc/robot/subsystems/PivotRhapsody.java +++ b/src/main/java/frc/robot/subsystems/PivotRhapsody.java @@ -157,7 +157,8 @@ public void simulationPeriodic() { LightningShuffleboard.setDouble("Pivot", "SimIntegral", simPivotPid.getI()); LightningShuffleboard.setDoubleArray("Pivot", "simPivotPose", () -> new double[] {simPivotPose.getX(), simPivotPose.getY(), simPivotPose.getZ(), - simPivotPose.getRotation().getX(), simPivotPose.getRotation().getY(), simPivotPose.getRotation().getZ()}); + Units.radiansToDegrees(simPivotPose.getRotation().getX()), Units.radiansToDegrees(simPivotPose.getRotation().getY()), + Units.radiansToDegrees(simPivotPose.getRotation().getZ())}); pivotSim.setInputVoltage(pivotPIDOutput * 12); diff --git a/src/main/java/frc/robot/subsystems/SimGamepeices.java b/src/main/java/frc/robot/subsystems/SimGamepeices.java index e4cdf2c7..474dcb23 100644 --- a/src/main/java/frc/robot/subsystems/SimGamepeices.java +++ b/src/main/java/frc/robot/subsystems/SimGamepeices.java @@ -6,11 +6,15 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.Constants.CollectorConstants; import frc.robot.Constants.FlywheelConstants; +import frc.robot.Constants.IndexerConstants; import frc.robot.Constants.PeiceSimConstants; import frc.robot.Constants.RhapsodyPivotConstants; import frc.thunder.shuffleboard.LightningShuffleboard; +import java.util.function.DoubleSupplier; public class SimGamepeices extends SubsystemBase{ public Peice[] peices; @@ -26,7 +30,7 @@ public class Peice { public Pose3d pose; public boolean isHeld; public double timeHeld; - public double peiceNum; + public double peiceNum; // for publishing public Peice(Pose3d pose, boolean isHeld, double peiceNum) { this.pose = pose; @@ -72,15 +76,20 @@ public SimGamepeices(PivotRhapsody pivot, Flywheel flywheel, Collector collector for (Peice peice : peices) { publish(peice); } + + addPeice(new Peice(new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)), 11)); + peices[11].isHeld = true; + heldPeice = peices[11]; } public void collect(){ if (heldPeice != null) { return; } - + for (Peice peice : peices) { - if (Math.hypot(peice.pose.getTranslation().getX(), peice.pose.getTranslation().getY()) + if (Math.hypot(drivetrain.getPose().getX() - peice.pose.getTranslation().getX(), + drivetrain.getPose().getY() - peice.pose.getTranslation().getY()) < PeiceSimConstants.COLLECT_DISTANCE && !peice.isHeld && collector.getPower() > 0 && drivetrain.getPose().getRotation().getDegrees() < PeiceSimConstants.COLLECT_ANGLE_MAX.getDegrees() && drivetrain.getPose().getRotation().getDegrees() > PeiceSimConstants.COLLECT_ANGLE_MIN.getDegrees()) { @@ -91,16 +100,45 @@ public void collect(){ } break; } + + } } - public void shoot(){ + public void simBeamBreaks(){ if (heldPeice == null) { return; } - if (indexer.getPower() > 0 && heldPeice != null && Utils.getCurrentTimeSeconds() - heldPeice.timeHeld > 1 - && flywheel.getTopMotorRPM() > 0 && flywheel.getBottomMotorRPM() > 0) - { + + double distanceFromStart = -0.5; + double timeAtCollection = Utils.getCurrentTimeSeconds(); + + while (distanceFromStart <= 1.5){ + double collectorSpeed = collector.getPower() * CollectorConstants.MaxAngularRate; + double indexerSpeed = indexer.getPower() * IndexerConstants.MaxAngularRate; + double flywheelSpeed = (flywheel.getBottomMotorRPM() + flywheel.getTopMotorRPM()) / 2 * FlywheelConstants.CIRCUMFRENCE / 60; + double t = Utils.getCurrentTimeSeconds() - timeAtCollection; + + if (distanceFromStart >= -0.5 && distanceFromStart < -0.25) { + // In Colelector only + distanceFromStart =+ t * collectorSpeed; + } else if (distanceFromStart >= -0.25 && distanceFromStart < 0.5) { + // In Collector and Indexer + distanceFromStart =+ (t * collectorSpeed + t * indexerSpeed) / 2; + } else if (distanceFromStart >= 0.5 && distanceFromStart < 1.5) { + // In Indexer and Flywheel + distanceFromStart =+ (t * indexerSpeed + t * flywheelSpeed) / 2; + } else { + // In Flywheel only + distanceFromStart =+ t * flywheelSpeed; + } + } + shoot(); + } + + public void shoot(){ + + System.out.println("Sim shooting"); heldPeice.isHeld = false; simShootTraj(heldPeice); heldPeice = null; @@ -129,19 +167,18 @@ public void simShootTraj(Peice peice){ // dx = vx * t dx = vx * t; - dx = dx * Math.cos(initialPose.getRotation().getDegrees()) + initialPose.getX(); dy = dx * Math.sin(initialPose.getRotation().getDegrees()) + initialPose.getY(); + dx = dx * Math.cos(initialPose.getRotation().getDegrees()) + initialPose.getX(); peice.pose = new Pose3d(dx, dy, dz, new Rotation3d(0, 0, initialPose.getRotation().getDegrees())); } } public void dispensePeiceFromSource(){ - if(dispensedPeice != null || DriverStation.isAutonomous() /*|| !DriverStation.isEnabled()*/){ + if(dispensedPeice != null || DriverStation.isAutonomous() || !DriverStation.isEnabled()){ return; } addPeice(new Peice(PeiceSimConstants.FROM_SOURCE, peices.length)); dispensedPeice = peices[peices.length - 1]; - publish(peices[peices.length - 1]); } public void updateHeldPeicePose(){ @@ -164,6 +201,7 @@ public void addPeice(Peice peice){ } newPeices[peices.length] = peice; peices = newPeices; + publish(peice); } public void deletePeice(Peice peice){ @@ -181,7 +219,8 @@ public void deletePeice(Peice peice){ public void publish(Peice peice){ LightningShuffleboard.setDoubleArray("Peices", "peice #" + peice.peiceNum, () -> new double[] {peice.pose.getX(), peice.pose.getY(), peice.pose.getZ(), - peice.pose.getRotation().getX(), peice.pose.getRotation().getY(), peice.pose.getRotation().getZ()}); + Units.radiansToDegrees(peice.pose.getRotation().getX()), Units.radiansToDegrees(peice.pose.getRotation().getY()), + Units.radiansToDegrees(peice.pose.getRotation().getZ())}); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index d03cc131..f64e9da3 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -20,6 +20,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.constraint.RectangularRegionConstraint; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; @@ -177,6 +178,9 @@ public void applyVisionPose(Pose4d pose) { * @return the request to drive for the drivetrain */ public Command applyPercentRequestField(DoubleSupplier x, DoubleSupplier y, DoubleSupplier rot) { + // if (RobotBase.isSimulation()){ + // return run(() -> this.setControl(driveField.withVelocityX(x.getAsDouble() * maxSpeed).withVelocityY(y.getAsDouble() * maxSpeed).withRotationalRate(rot.getAsDouble() * maxAngularRate))); + // } return run(() -> this.setControl(driveField.withVelocityX(x.getAsDouble() * maxSpeed).withVelocityY(y.getAsDouble() * maxSpeed).withRotationalRate(rot.getAsDouble() * maxAngularRate).withDriveRequestType(DriveRequestType.Velocity))); } @@ -188,7 +192,11 @@ public Command applyPercentRequestField(DoubleSupplier x, DoubleSupplier y, Doub * @param rot the rotational velocity in rad/s */ public void setField(double x, double y, double rot) { - this.setControl(driveField.withVelocityX(x).withVelocityY(y).withRotationalRate(rot).withDriveRequestType(DriveRequestType.Velocity)); + if (RobotBase.isSimulation()){ + this.setControl(driveField.withVelocityX(x).withVelocityY(y).withRotationalRate(rot)); + } else { + this.setControl(driveField.withVelocityX(x).withVelocityY(y).withRotationalRate(rot).withDriveRequestType(DriveRequestType.Velocity)); + } } /** @@ -199,7 +207,11 @@ public void setField(double x, double y, double rot) { * @param rot the rotational, percent of max velocity rad/s */ public void setFieldDriver(double x, double y, double rot) { - this.setControl(driveField.withVelocityX(x * maxSpeed).withVelocityY(y * maxSpeed).withRotationalRate(rot).withDriveRequestType(DriveRequestType.Velocity)); + if (RobotBase.isSimulation()){ + this.setControl(driveField.withVelocityX(x * maxSpeed).withVelocityY(y * maxSpeed).withRotationalRate(rot)); + } else { + this.setControl(driveField.withVelocityX(x * maxSpeed).withVelocityY(y * maxSpeed).withRotationalRate(rot).withDriveRequestType(DriveRequestType.Velocity)); + } } public void setFieldDriver2(double x, double y, double rot) { @@ -215,6 +227,9 @@ public void setFieldDriver2(double x, double y, double rot) { * @return the request to drive for the drivetrain */ public Command applyPercentRequestRobot(DoubleSupplier x, DoubleSupplier y, DoubleSupplier rot) { + if (RobotBase.isSimulation()){ + return run(() -> this.setControl(driveRobot.withVelocityX(x.getAsDouble() * maxSpeed).withVelocityY(y.getAsDouble() * maxSpeed).withRotationalRate(rot.getAsDouble() * maxAngularRate))); + } return run(() -> this.setControl(driveRobot.withVelocityX(x.getAsDouble() * maxSpeed).withVelocityY(y.getAsDouble() * maxSpeed).withRotationalRate(rot.getAsDouble() * maxAngularRate).withDriveRequestType(DriveRequestType.Velocity))); } @@ -226,7 +241,11 @@ public Command applyPercentRequestRobot(DoubleSupplier x, DoubleSupplier y, Doub * @param rot the rotational velocity in rad/s */ public void setRobot(double x, double y, double rot) { - this.setControl(driveRobot.withVelocityX(x).withVelocityY(y).withRotationalRate(rot).withDriveRequestType(DriveRequestType.Velocity)); + if (RobotBase.isSimulation()){ + this.setControl(driveRobot.withVelocityX(x).withVelocityY(y).withRotationalRate(rot)); + } else { + this.setControl(driveRobot.withVelocityX(x).withVelocityY(y).withRotationalRate(rot).withDriveRequestType(DriveRequestType.Velocity)); + } } /** diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index 103bc7c3..de65c9c5 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit 103bc7c33630fd0d3ca50873deef06aa78aa4294 +Subproject commit de65c9c5fc65db7237217b0d724e33788ef53fc4