Skip to content

Commit

Permalink
quick houston changes
Browse files Browse the repository at this point in the history
  • Loading branch information
MOE 365 Programming Laptop authored and MOE 365 Programming Laptop committed Apr 19, 2024
1 parent 7356cd1 commit 327df5c
Show file tree
Hide file tree
Showing 4 changed files with 103 additions and 1 deletion.
1 change: 1 addition & 0 deletions src/main/java/frc/robot/FortissiMOEContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -285,6 +285,7 @@ public FortissiMOEContainer() {
m_chooser.addOption("driveForward", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).rollOutAuto());
m_chooser.addOption("3 Note Centerline Auto (DC3C2)", new tripleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).DC3C2());
m_chooser.addOption("2 Note Centerline Auto Obj Detect", new tripleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0,0).DC3ObjDetect());
m_chooser.addOption("CenterLine Pass Auto Obj Detect (DC5C4PassC3OD)", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).DC5C4PassC3ObjDet());
SmartDashboard.putData("chooser", m_chooser);
}

Expand Down
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/commands/AutoDriveToNoteCommand.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package frc.robot.commands;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
Expand Down Expand Up @@ -55,7 +56,10 @@ public void execute() {
updateTarget();
if (target == null){
idleLoopCount += 1;
return;
}
// Put the detection on NetworkTables, for debugging
subsystem.field.getObject("NoteTarget").setPose(new Pose2d(target, new Rotation2d()));
// Drive towards target
var robotPose = subsystem.getEstimatedPose();
var delta = target.minus(robotPose.getTranslation());
Expand Down
97 changes: 97 additions & 0 deletions src/main/java/frc/robot/commands/autos/doubleNoteAutos.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -341,6 +341,103 @@ public Command DC5C4PassC3(){
Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2),
Commands.runOnce(()->swerveDrive.stopModules()));
}
public Command DC5C4PassC3ObjDet(){
Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointD, UsefulPoints.Points.RotationOfStartingPointD);
Pose2d endPose = new Pose2d(UsefulPoints.Points.CenterNote5,Rotation2d.fromDegrees(0));
Pose2d startPose2 = new Pose2d(endPose.getTranslation(),Rotation2d.fromDegrees(90));
Pose2d endPose2 = new Pose2d(UsefulPoints.Points.CenterNote4,Rotation2d.fromDegrees(90));
Pose2d startPose3 = new Pose2d(endPose2.getTranslation(),Rotation2d.fromDegrees(90));
Pose2d endPose3 = new Pose2d(UsefulPoints.Points.CenterNote5,Rotation2d.fromDegrees(-25));
Pose2d startPose4 = new Pose2d(endPose3.getTranslation(),Rotation2d.fromDegrees(90));
Pose2d endPose4 = new Pose2d(UsefulPoints.Points.CenterNote3,Rotation2d.fromDegrees(90));
Pose2d startPose5 = new Pose2d(endPose4.getTranslation(),Rotation2d.fromDegrees(90));
Pose2d endPose5 = new Pose2d(UsefulPoints.Points.CenterNote5,Rotation2d.fromDegrees(-25));

ArrayList<Translation2d> internalPoints = new ArrayList<Translation2d>();
Command trajCommand = swerveDrive.generateTrajectory(startPose,endPose,internalPoints,0,0);
Command trajCommand2 = swerveDrive.generateTrajectory(startPose2,endPose2,internalPoints,0,0);
Command trajCommand3 = swerveDrive.generateTrajectory(startPose3,endPose3,internalPoints,0,0);
Command trajCommand4 = swerveDrive.generateTrajectory(startPose4,endPose4,internalPoints,0,0);

Command shootNote = new shootSpeakerCommand(shooter,collector);
Command passNote = new NoteFeed(shooter,collector,()->1800);
Command passAnotherNote = new NoteFeed(shooter,collector,()->1800);
Command collectNote = new Collect(collector,.4,false);
Command collectAnotherNote = new Collect(collector,.4,false);
Command collectLastNote = new Collect(collector,.4,false);
Command headingCorrect = new setHeading(swerveDrive, ()-> 0.0, ()-> 0.0, ()->AllianceFlip.apply(Rotation2d.fromDegrees(-25)));
Command headingCorrectCollect = new setHeading(swerveDrive,()->0.0,()->0.0,()->AllianceFlip.apply(Rotation2d.fromDegrees(90)));
Command headingCorrect2 = new setHeading(swerveDrive,()->0.0,()->0.0,()->AllianceFlip.apply(Rotation2d.fromDegrees(-25)));
Command headingCorrectCollect2 = new setHeading(swerveDrive,()->0.0,()->0.0,()->AllianceFlip.apply(Rotation2d.fromDegrees(90)));
Command headingCorrect3 = new setHeading(swerveDrive,()->0.0,()->0.0,()->AllianceFlip.apply(Rotation2d.fromDegrees(180)));

var driveToNote = new DriveToNoteCommand(
swerveDrive,
vision,
// () -> Math.max(0, Math.hypot(driverJoystick.getRawAxis(0), driverJoystick.getRawAxis(1))-.05)*(maxMPS),
() -> 1.0,
() -> 0.0,
() -> 0.0,
(rumblePercent) -> {
SmartDashboard.putNumber("JoyRumble", rumblePercent);
//driverJoystick.setRumble(PS5Controller.RumbleType.kBothRumble, rumblePercent); //TODO: try different rumble types.
},
.56
);
var driveToNoteAgain = new DriveToNoteCommand(
swerveDrive,
vision,
// () -> Math.max(0, Math.hypot(driverJoystick.getRawAxis(0), driverJoystick.getRawAxis(1))-.05)*(maxMPS),
() -> 1.0,
() -> 0.0,
() -> 0.0,
(rumblePercent) -> {
SmartDashboard.putNumber("JoyRumble", rumblePercent);
//driverJoystick.setRumble(PS5Controller.RumbleType.kBothRumble, rumblePercent); //TODO: try different rumble types.
},
.56
);

var driveToLastNote = new DriveToNoteCommand(
swerveDrive,
vision,
// () -> Math.max(0, Math.hypot(driverJoystick.getRawAxis(0), driverJoystick.getRawAxis(1))-.05)*(maxMPS),
() -> 1.0,
() -> 0.0,
() -> 0.0,
(rumblePercent) -> {
SmartDashboard.putNumber("JoyRumble", rumblePercent);
//driverJoystick.setRumble(PS5Controller.RumbleType.kBothRumble, rumblePercent); //TODO: try different rumble types.
},
.56
);


return Commands.sequence(
swerveDrive.setInitPosition(startPose),
Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)).andThen(Commands.waitSeconds(.15)),
Commands.race(shootNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),
Commands.race(Commands.parallel(trajCommand.andThen(driveToNote.withTimeout(2))
.andThen(()->swerveDrive.stopModules()), collectNote),
Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(6),
Commands.runOnce(()->swerveDrive.stopModules()),//goes to point C5
Commands.race(headingCorrect.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),
Commands.race(passNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), //heading correct and spit back
Commands.race(headingCorrectCollect.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), //swap back to 90
Commands.race(Commands.parallel(trajCommand2.andThen(driveToNoteAgain.withTimeout(2))
.andThen(()->swerveDrive.stopModules()), collectNote),
Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(6),
Commands.runOnce(()->swerveDrive.stopModules()), //goes to point C4
Commands.race(Commands.parallel(trajCommand3.andThen(()->swerveDrive.stopModules())),
Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2),
Commands.runOnce(()->swerveDrive.stopModules()),Commands.race(headingCorrect2.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),
Commands.race(passAnotherNote,Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))), //spit out and then reorient to collect
Commands.race(headingCorrectCollect2.andThen(()-> swerveDrive.stopModules()),Commands.run(()-> armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),
Commands.race(Commands.parallel(trajCommand4.andThen(driveToLastNote.withTimeout(2))
.andThen(()->swerveDrive.stopModules()), collectNote),
Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(6),
Commands.runOnce(()->swerveDrive.stopModules()));
}
public Command DoubleNoteAuto3(){//TODO: Fix coordinates, create actual shoot and collect commands

Rotation2d startRotation = new Rotation2d(0);
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/vision/Vision.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ public Optional<TimestampedPose> getAprilTagPose() {
}

public List<Translation2d> detections(){
ArrayList<Translation2d> dets = new ArrayList<>();
List<Translation2d> dets = new ArrayList<>();
var entry = subNotes.getAtomic();
if (entry.timestamp > oldDetTime && entry.value.length %3 == 0){
oldDetTime = entry.timestamp;
Expand Down

0 comments on commit 327df5c

Please sign in to comment.