generated from Team488/FRCRobotTemplate
-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'main' into PrepareToScoreInAmpCommand
- Loading branch information
Showing
7 changed files
with
194 additions
and
47 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
102 changes: 102 additions & 0 deletions
102
src/main/java/competition/simulation/Simulator2024.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,102 @@ | ||
| package competition.simulation; | ||
|
|
||
| import com.revrobotics.CANSparkBase; | ||
| import competition.subsystems.arm.ArmSubsystem; | ||
| import competition.subsystems.drive.DriveSubsystem; | ||
| import competition.subsystems.pose.PoseSubsystem; | ||
| import competition.subsystems.shooter.ShooterWheelSubsystem; | ||
| import edu.wpi.first.math.geometry.Pose2d; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.geometry.Translation2d; | ||
| import xbot.common.controls.actuators.mock_adapters.MockCANSparkMax; | ||
| import xbot.common.math.MovingAverageForDouble; | ||
| import xbot.common.math.MovingAverageForTranslation2d; | ||
|
|
||
| import javax.inject.Inject; | ||
|
|
||
| public class Simulator2024 { | ||
|
|
||
| MovingAverageForTranslation2d translationAverageCalculator = | ||
| new MovingAverageForTranslation2d(15); | ||
| MovingAverageForDouble rotationAverageCalculator = | ||
| new MovingAverageForDouble(15); | ||
| MovingAverageForDouble leftArmPositionCalculator = | ||
| new MovingAverageForDouble(15); | ||
| MovingAverageForDouble rightArmPositionCalculator = | ||
| new MovingAverageForDouble(15); | ||
| MovingAverageForDouble shooterVelocityCalculator = | ||
| new MovingAverageForDouble(50); | ||
|
|
||
| private final PoseSubsystem pose; | ||
| private final DriveSubsystem drive; | ||
| private final ArmSubsystem arm; | ||
| private final ShooterWheelSubsystem shooter; | ||
|
|
||
| @Inject | ||
| public Simulator2024(PoseSubsystem pose, DriveSubsystem drive, | ||
| ArmSubsystem arm, ShooterWheelSubsystem shooter) { | ||
| this.pose = pose; | ||
| this.drive = drive; | ||
| this.arm = arm; | ||
| this.shooter = shooter; | ||
| } | ||
|
|
||
|
|
||
|
|
||
| public void update() { | ||
| double robotTopSpeedInMetersPerSecond = 3.0; | ||
| double robotLoopPeriod = 0.02; | ||
| double poseAdjustmentFactorForSimulation = robotTopSpeedInMetersPerSecond * robotLoopPeriod; | ||
|
|
||
| double robotTopAngularSpeedInDegreesPerSecond = 360; | ||
| double headingAdjustmentFactorForSimulation = robotTopAngularSpeedInDegreesPerSecond * robotLoopPeriod; | ||
|
|
||
|
|
||
| var currentPose = pose.getCurrentPose2d(); | ||
|
|
||
| // Extremely simple physics simulation. We want to give the robot some very basic translational and rotational | ||
| // inertia. We can take the moving average of the last second or so of robot commands and apply that to the | ||
| // robot's pose. This is a very simple way to simulate the robot's movement without having to do any real physics. | ||
|
|
||
| translationAverageCalculator.add(drive.lastRawCommandedDirection); | ||
| var currentAverage = translationAverageCalculator.getAverage(); | ||
|
|
||
| rotationAverageCalculator.add(drive.lastRawCommandedRotation); | ||
| var currentRotationAverage = rotationAverageCalculator.getAverage(); | ||
|
|
||
| var updatedPose = new Pose2d( | ||
| new Translation2d( | ||
| currentPose.getTranslation().getX() + currentAverage.getX() * poseAdjustmentFactorForSimulation, | ||
| currentPose.getTranslation().getY() + currentAverage.getY() * poseAdjustmentFactorForSimulation), | ||
| currentPose.getRotation().plus(Rotation2d.fromDegrees(currentRotationAverage * headingAdjustmentFactorForSimulation))); | ||
| pose.setCurrentPoseInMeters(updatedPose); | ||
|
|
||
| // Let's also have a very simple physics mock for the arm and the shooter. | ||
| // Get the power or setpoint for each arm. | ||
| double powerToTicksRatio = 1; | ||
| var leftMockMotor = ((MockCANSparkMax)arm.armMotorLeft); | ||
| var rightMockMotor = ((MockCANSparkMax)arm.armMotorRight); | ||
|
|
||
| leftMockMotor.setPosition(leftMockMotor.getPosition() + (leftMockMotor.get() * powerToTicksRatio)); | ||
| rightMockMotor.setPosition(rightMockMotor.getPosition() + (rightMockMotor.get() * powerToTicksRatio)); | ||
|
|
||
| // They might be using PID to control the arm. If so, we can use a moving aveage of their setpoint | ||
| // to approximate internal PID. | ||
| leftArmPositionCalculator.add(leftMockMotor.getReference()); | ||
| rightArmPositionCalculator.add(rightMockMotor.getReference()); | ||
|
|
||
| if (leftMockMotor.getControlType() == CANSparkBase.ControlType.kPosition) { | ||
| leftMockMotor.setPosition(leftArmPositionCalculator.getAverage()); | ||
| } | ||
| if (rightMockMotor.getControlType() == CANSparkBase.ControlType.kPosition) { | ||
| rightMockMotor.setPosition(rightArmPositionCalculator.getAverage()); | ||
| } | ||
|
|
||
| // The shooter wheel should pretty much always be in velocity mode. | ||
| var shooterMockMotor = (MockCANSparkMax)shooter.leader; | ||
| shooterVelocityCalculator.add(shooterMockMotor.getReference()); | ||
| if (shooterMockMotor.getControlType() == CANSparkBase.ControlType.kVelocity) { | ||
| shooterMockMotor.setVelocity(shooterVelocityCalculator.getAverage()); | ||
| } | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
38 changes: 38 additions & 0 deletions
38
src/main/java/competition/subsystems/shooter/commands/FireWhenReadyCommand.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,38 @@ | ||
| package competition.subsystems.shooter.commands; | ||
|
|
||
| import competition.subsystems.arm.ArmSubsystem; | ||
| import competition.subsystems.collector.CollectorSubsystem; | ||
| import competition.subsystems.shooter.ShooterWheelSubsystem; | ||
| import xbot.common.command.BaseCommand; | ||
|
|
||
| import javax.inject.Inject; | ||
|
|
||
| public class FireWhenReadyCommand extends BaseCommand { | ||
| final ShooterWheelSubsystem wheel; | ||
| final ArmSubsystem arm; | ||
| final CollectorSubsystem collector; | ||
|
|
||
| @Inject | ||
| public FireWhenReadyCommand(ShooterWheelSubsystem wheel, ArmSubsystem arm, CollectorSubsystem collector) { | ||
| this.wheel = wheel; | ||
| this.arm = arm; | ||
| this.collector = collector; | ||
| } | ||
|
|
||
| @Override | ||
| public void initialize() { | ||
| log.info("Initializing..."); | ||
| } | ||
|
|
||
| @Override | ||
| public void execute() { | ||
| /* WE CANNOT CHECK IF CURRENT VALUE OF WHEEL AND TARGET VALUE ARE EXACTLY THE SAME, THAT IS WHY WE | ||
| HAVE THE TOLERANCE PROPERTIES IN THE FIRST PLACE | ||
| RUNS 50 TIMES A SECOND | ||
| */ | ||
| if (wheel.isMaintainerAtGoal() && arm.isMaintainerAtGoal()) { | ||
| collector.fire(); | ||
| } | ||
| } | ||
| } |