Skip to content

Commit

Permalink
[#208] sim beambreaks
Browse files Browse the repository at this point in the history
  • Loading branch information
@Vilok1
Vilok1 committed Aug 28, 2024
1 parent f799612 commit 020a193
Show file tree
Hide file tree
Showing 10 changed files with 101 additions and 28 deletions.
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -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;
}
}

8 changes: 4 additions & 4 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
11 changes: 7 additions & 4 deletions src/main/java/frc/robot/command/shoot/NotePass.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Expand Down Expand Up @@ -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);
}
}

Expand Down
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/command/shoot/SmartShoot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/subsystems/Collector.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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,
Expand All @@ -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();
Expand Down
7 changes: 5 additions & 2 deletions src/main/java/frc/robot/subsystems/Indexer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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);

Expand Down Expand Up @@ -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);
}
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/subsystems/PivotRhapsody.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
61 changes: 50 additions & 11 deletions src/main/java/frc/robot/subsystems/SimGamepeices.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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()) {
Expand All @@ -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;
Expand Down Expand Up @@ -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(){
Expand All @@ -164,6 +201,7 @@ public void addPeice(Peice peice){
}
newPeices[peices.length] = peice;
peices = newPeices;
publish(peice);
}

public void deletePeice(Peice peice){
Expand All @@ -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())});
}


Expand Down
25 changes: 22 additions & 3 deletions src/main/java/frc/robot/subsystems/Swerve.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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)));
}

Expand All @@ -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));
}
}

/**
Expand All @@ -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) {
Expand All @@ -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)));
}

Expand All @@ -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));
}
}

/**
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/thunder
Submodule thunder updated 1 files
+65 −0 hardware/LightningBeamBreak.java

0 comments on commit 020a193

Please sign in to comment.