generated from StuyPulse/Phil
-
Notifications
You must be signed in to change notification settings - Fork 8
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #2 from StuyPulse/JanTenth
Jan tenth
- Loading branch information
Showing
18 changed files
with
515 additions
and
62 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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1 @@ | ||
| [] |
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,92 @@ | ||
| { | ||
| "keyboardJoysticks": [ | ||
| { | ||
| "axisConfig": [ | ||
| { | ||
| "decKey": 65, | ||
| "incKey": 68 | ||
| }, | ||
| { | ||
| "decKey": 87, | ||
| "incKey": 83 | ||
| }, | ||
| { | ||
| "decKey": 69, | ||
| "decayRate": 0.0, | ||
| "incKey": 82, | ||
| "keyRate": 0.009999999776482582 | ||
| } | ||
| ], | ||
| "axisCount": 3, | ||
| "buttonCount": 4, | ||
| "buttonKeys": [ | ||
| 90, | ||
| 88, | ||
| 67, | ||
| 86 | ||
| ], | ||
| "povConfig": [ | ||
| { | ||
| "key0": 328, | ||
| "key135": 323, | ||
| "key180": 322, | ||
| "key225": 321, | ||
| "key270": 324, | ||
| "key315": 327, | ||
| "key45": 329, | ||
| "key90": 326 | ||
| } | ||
| ], | ||
| "povCount": 1 | ||
| }, | ||
| { | ||
| "axisConfig": [ | ||
| { | ||
| "decKey": 74, | ||
| "incKey": 76 | ||
| }, | ||
| { | ||
| "decKey": 73, | ||
| "incKey": 75 | ||
| } | ||
| ], | ||
| "axisCount": 2, | ||
| "buttonCount": 4, | ||
| "buttonKeys": [ | ||
| 77, | ||
| 44, | ||
| 46, | ||
| 47 | ||
| ], | ||
| "povCount": 0 | ||
| }, | ||
| { | ||
| "axisConfig": [ | ||
| { | ||
| "decKey": 263, | ||
| "incKey": 262 | ||
| }, | ||
| { | ||
| "decKey": 265, | ||
| "incKey": 264 | ||
| } | ||
| ], | ||
| "axisCount": 2, | ||
| "buttonCount": 6, | ||
| "buttonKeys": [ | ||
| 260, | ||
| 268, | ||
| 266, | ||
| 261, | ||
| 269, | ||
| 267 | ||
| ], | ||
| "povCount": 0 | ||
| }, | ||
| { | ||
| "axisCount": 0, | ||
| "buttonCount": 0, | ||
| "povCount": 0 | ||
| } | ||
| ] | ||
| } |
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,39 @@ | ||
| { | ||
| "NTProvider": { | ||
| "types": { | ||
| "/FMSInfo": "FMSInfo", | ||
| "/SmartDashboard/Autonomous": "String Chooser", | ||
| "/SmartDashboard/Field": "Field2d", | ||
| "/SmartDashboard/Odometry/Field": "Field2d", | ||
| "/SmartDashboard/Sim Field": "Field2d" | ||
| }, | ||
| "windows": { | ||
| "/SmartDashboard/Autonomous": { | ||
| "window": { | ||
| "visible": true | ||
| } | ||
| }, | ||
| "/SmartDashboard/Sim Field": { | ||
| "window": { | ||
| "visible": true | ||
| } | ||
| } | ||
| } | ||
| }, | ||
| "NetworkTables": { | ||
| "transitory": { | ||
| "SmartDashboard": { | ||
| "Sim Field": { | ||
| "open": true | ||
| }, | ||
| "SimDrivetrain": { | ||
| "open": true | ||
| }, | ||
| "open": true | ||
| } | ||
| } | ||
| }, | ||
| "NetworkTables Info": { | ||
| "visible": true | ||
| } | ||
| } |
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
87 changes: 87 additions & 0 deletions
87
src/main/java/com/stuypulse/robot/commands/AngleAlign.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,87 @@ | ||
| package com.stuypulse.robot.commands; | ||
|
|
||
| import com.stuypulse.robot.constants.Settings.Alignment; | ||
| import com.stuypulse.robot.subsystems.drivetrain.AbstractDrivetrain; | ||
| import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; | ||
| import com.stuypulse.robot.subsystems.odometry.Odometry; | ||
| import com.stuypulse.robot.subsystems.vision.AbstractVision; | ||
| import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; | ||
| import com.stuypulse.stuylib.math.Angle; | ||
| import com.stuypulse.stuylib.streams.angles.AFuser; | ||
| import com.stuypulse.stuylib.streams.booleans.BStream; | ||
| import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; | ||
| import com.stuypulse.stuylib.streams.numbers.filters.IFilter; | ||
| import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.Command; | ||
|
|
||
| public class AngleAlign extends Command { | ||
| private final AbstractDrivetrain drivetrain; | ||
| private final AbstractOdometry odometry; | ||
| private final AbstractVision vision; | ||
|
|
||
| private final AFuser angleError; | ||
| private IFilter speedAdjFilter; | ||
|
|
||
| protected final AnglePIDController angleController; | ||
|
|
||
| private final BStream finished; | ||
|
|
||
| public AngleAlign() { | ||
| this.drivetrain = AbstractDrivetrain.getInstance(); | ||
| this.odometry = Odometry.getInstance(); | ||
| this.vision = AbstractVision.getInstance(); | ||
|
|
||
| this.angleError = new AFuser(Alignment.FUSION_FILTER.get(), | ||
| () -> Angle.fromRotation2d(vision.getOutput().get(0).getPrimaryTag().getPose().toPose2d().getRotation()), | ||
| () -> Angle.fromRotation2d(odometry.getPose().getRotation()) | ||
| ); | ||
|
|
||
| this.speedAdjFilter = new LowPassFilter(Alignment.SPEED_ADJ_FILTER.get()); | ||
|
|
||
| this.angleController = new AnglePIDController(Alignment.Rotation.P, Alignment.Rotation.I, Alignment.Rotation.D); | ||
|
|
||
| this.finished = BStream.create(this::isAligned) | ||
| //TODO: check to see if it is degrees or radians | ||
| .and(() -> angleController.isDoneDegrees(Alignment.ALIGNED_THRESHOLD_ANGLE.get())) | ||
| .filtered(new BDebounceRC.Rising(Alignment.DEBOUNCE_TIME)); | ||
|
|
||
| addRequirements(drivetrain, odometry, vision); | ||
| } | ||
|
|
||
| public boolean isAligned() { | ||
| // check to see if the robot is within a threshold of the april tag target | ||
| return Math.abs(angleError.get().toDegrees()) < Alignment.ALIGNED_THRESHOLD_ANGLE.get(); | ||
| } | ||
|
|
||
| private double getTurn() { | ||
| // //calcs with the setpoint angle of line between tag and robot to the current angle of the robot | ||
| // return angleController.update( | ||
| // /* angle of line between tag and robot*/ | ||
| // angleController.getSetpoint() | ||
| // , | ||
| // odometry.getRotation()); | ||
| return 0; | ||
| } | ||
|
|
||
| @Override | ||
| public void initialize() { | ||
| odometry.resetOdometery(odometry.getPose()); | ||
| angleController.reset(); | ||
| angleError.reset(); | ||
| } | ||
|
|
||
| @Override | ||
| public void execute() { | ||
| } | ||
|
|
||
| @Override | ||
| public boolean isFinished() { | ||
| return finished.get(); | ||
| } | ||
|
|
||
| @Override | ||
| public void end(boolean interrupted) { | ||
| drivetrain.stop(); | ||
| } | ||
| } |
25 changes: 25 additions & 0 deletions
25
src/main/java/com/stuypulse/robot/commands/drivetrain/DriveDrive.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,25 @@ | ||
| package com.stuypulse.robot.commands.drivetrain; | ||
|
|
||
| import com.stuypulse.robot.subsystems.drivetrain.AbstractDrivetrain; | ||
| import com.stuypulse.stuylib.input.Gamepad; | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.Command; | ||
|
|
||
| // DriveCommand.java | ||
| public class DriveDrive extends Command { | ||
|
|
||
| private AbstractDrivetrain drivetrain; | ||
| private Gamepad gamepad; | ||
|
|
||
| public DriveDrive(AbstractDrivetrain drivetrain, Gamepad gamepad) { | ||
| this.drivetrain = drivetrain; | ||
| this.gamepad = gamepad; | ||
|
|
||
| addRequirements(drivetrain); | ||
| } | ||
|
|
||
| public void execute() { | ||
| drivetrain.tankDriveVolts(gamepad.getLeftTrigger(), gamepad.getRightTrigger()); | ||
| } | ||
|
|
||
| } |
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
Oops, something went wrong.