Skip to content

Commit

Permalink
lehigh 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 5, 2024
1 parent cc1625d commit 51f91b0
Show file tree
Hide file tree
Showing 5 changed files with 27 additions and 23 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,6 @@ public final class Constants {
public static class OperatorConstants {
public static final int kDriverControllerPort = 0;
}
public static Rotation2d collectorShoulder = Rotation2d.fromDegrees(84.4);
public static Rotation2d collectorShoulder = Rotation2d.fromDegrees(83.6);
public static Rotation2d collectorWrist = Rotation2d.fromDegrees(-45);
}
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/FortissiMOEContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ public class FortissiMOEContainer{
double width = Units.inchesToMeters(14);
double length = Units.inchesToMeters(14);
double maxMPS = 174/39.3701;
double maxMPSAuto = 3;
double maxMPSAuto = 4;
double maxRPS = 1.5*2*Math.PI;
double maxRPS2 = Math.PI;

Expand Down Expand Up @@ -129,12 +129,12 @@ public class FortissiMOEContainer{

/////////////////////////////////////////////////////////////////////////////arm subsystem start
private final Arm armSubsystem = new Arm(4, 15,14, 35, 37,
24.0e-2, 24.0e-3, 48.0e-4, .1724,3.0e-2, 2.0e-3,
24.0e-2, 24.0e-3, 48.0e-4, .1724,0/*3.0e-2*/, 2.0e-3,
.0185, .14833, 1.42e-4, 1.36e-4,
.95908/3, .54837/3, .033244/3, .00498/3,
.6048, .3615, .18133,.14154,10.725,27.837,
5.6705,5.899,0,0,
Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(-60), Rotation2d.fromDegrees(120), Rotation2d.fromDegrees(-60),
Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(-60), Rotation2d.fromDegrees(123), Rotation2d.fromDegrees(-60),
100,300);

/////////////////////////////////////////////////////////////////////////// arm subsystem end
Expand Down Expand Up @@ -224,7 +224,7 @@ public class FortissiMOEContainer{
SmartDashboard.putNumber("Roll", pigeon.getRoll());
SmartDashboard.putNumber("Pitch", pigeon.getPitch());
pdh.setSwitchableChannel((collectorSubsystem.isCollected() && ((System.currentTimeMillis()/100)%2 == 0))
|| (shooterSubsystem.shooterAtSpeed() && shooterSubsystem.getDesiredTopSpeed() != 0));
|| (shooterSubsystem.shooterAtSpeed() && shooterSubsystem.getDesiredTopSpeed() != 0 && collectorSubsystem.isCollected()));
});
//weirdest command ever - climbing & pdh logic

Expand Down Expand Up @@ -321,7 +321,7 @@ private void configureBindings() {
|| wristUp.getAsBoolean() || buttonBox.getRawButton(3))));
//amp shot

new JoystickButton(functionJoystick, 3).onTrue(Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(111), Rotation2d.fromDegrees(-110)), Set.of(armSubsystem))
new JoystickButton(functionJoystick, 3).onTrue(Commands.defer(()->armSubsystem.goToPoint(Rotation2d.fromDegrees(113), Rotation2d.fromDegrees(-110)), Set.of(armSubsystem))
.until(()->(shoulderUp.getAsBoolean() || functionJoystick.getRawButton(10) ||
functionJoystick.getRawButton(2) || shoulderDown.getAsBoolean() ||
functionJoystick.getRawButton(1) || functionJoystick.getRawButton(8)|| wristDown.getAsBoolean()
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/commands/DriveToNoteCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,10 +83,10 @@ public void execute() {
var unitDelta = delta.div(delta.getNorm());//.times(speedSupplier.getAsDouble());

var robotAngle = unitDelta.getAngle();
if (delta.getNorm() <= 24) robotAngle = robotPose.getRotation().times(-1);
if (delta.getNorm() <= Units.inchesToMeters(24)) robotAngle = robotPose.getRotation().times(1);
SmartDashboard.putNumber("robot Object Detection angle", robotAngle.getDegrees());
// var yawOffset = subsystem.getRotation2d().minus(robotPose.getRotation());
subsystem.setDesiredYaw(-robotAngle.getDegrees());//Set absolute heading
subsystem.setDesiredYaw(robotAngle.getDegrees());//Set absolute heading

var speedVal = speedSupplier.getAsDouble();
if (speedVal < .1) speedVal = 0;
Expand Down
15 changes: 8 additions & 7 deletions src/main/java/frc/robot/commands/autos/tripleNoteAutos.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -210,21 +210,21 @@ public Command CW1W2W3(){//TODO: Fix coordinates, create actual shoot and collec
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(()->swerveDrive.stopModules()), collectNote),
Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(4),
Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(3),
Commands.runOnce(()->swerveDrive.stopModules()),
Commands.race(trajCommand2.andThen(()->swerveDrive.stopModules()),
Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),
Commands.runOnce(()->swerveDrive.stopModules()),
Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),
Commands.race(Commands.parallel(trajCommand3.andThen(()->swerveDrive.stopModules()), collectNote2),
Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(4),
Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(3),
Commands.runOnce(()->swerveDrive.stopModules()),
Commands.race(Commands.parallel(trajCommand4.andThen(()->swerveDrive.stopModules())),
Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),
Commands.runOnce(()->swerveDrive.stopModules()),
Commands.race(shootLastNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),
Commands.race(Commands.parallel(trajCommand5.andThen(()->swerveDrive.stopModules()), collectNote3),
Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(4),
Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(3),
Commands.runOnce(()->swerveDrive.stopModules()),
Commands.race(trajCommand6.andThen(()->swerveDrive.stopModules()),
Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))),
Expand All @@ -238,10 +238,11 @@ public Command DC3C2(){//TODO: Fix coordinates, create actual shoot and collect
//traj 1
Pose2d startPose = new Pose2d(UsefulPoints.Points.StartingPointD, UsefulPoints.Points.RotationOfStartingPointD);
Rotation2d endRotation = new Rotation2d(0);
Translation2d endTranslation = new Translation2d(UsefulPoints.Points.CenterNote3.getX()-Units.inchesToMeters(12),
UsefulPoints.Points.CenterNote3.getY()+Units.inchesToMeters(10));
Translation2d endTranslation = new Translation2d(UsefulPoints.Points.CenterNote3.getX()-Units.inchesToMeters(6),
UsefulPoints.Points.CenterNote3.getY());
Pose2d endPose = new Pose2d(endTranslation, endRotation); //goes from start D to C3


Translation2d endTranslation2 = UsefulPoints.Points.StageEnterBottom;
// Rotation2d startRotation2 = new Rotation2d(swerveDrive.getYaw());
Pose2d startPose2 = new Pose2d(endTranslation, endRotation);
Expand Down Expand Up @@ -314,7 +315,7 @@ public Command DC3C2(){//TODO: Fix coordinates, create actual shoot and collect
Commands.runOnce(()->swerveDrive.stopModules()),
Commands.defer(()->armSubsystem.goToPoint(
Rotation2d.fromDegrees(armSubsystem.autoAim(swerveDrive::getEstimatedPose).getX()),
Rotation2d.fromDegrees(armSubsystem.autoAim(swerveDrive::getEstimatedPose).getY())), Set.of(armSubsystem))
Rotation2d.fromDegrees(armSubsystem.autoAim(swerveDrive::getEstimatedPose).getY()-2)), Set.of(armSubsystem))
.andThen(Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState())).withTimeout(1)),
Commands.race(shootAnotherNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState()))).withTimeout(2),
Commands.defer(()->armSubsystem.goToPoint(Constants.collectorShoulder, Constants.collectorWrist), Set.of(armSubsystem)).andThen(Commands.waitSeconds(1)),
Expand All @@ -326,7 +327,7 @@ public Command DC3C2(){//TODO: Fix coordinates, create actual shoot and collect
Commands.runOnce(()->swerveDrive.stopModules()),
Commands.defer(()->armSubsystem.goToPoint(
Rotation2d.fromDegrees(armSubsystem.autoAim(swerveDrive::getEstimatedPose).getX()),
Rotation2d.fromDegrees(armSubsystem.autoAim(swerveDrive::getEstimatedPose).getY())), Set.of(armSubsystem)).andThen
Rotation2d.fromDegrees(armSubsystem.autoAim(swerveDrive::getEstimatedPose).getY()-2)), Set.of(armSubsystem)).andThen
(Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState())).withTimeout(1)),
Commands.parallel(shootLastNote, Commands.run(()->armSubsystem.holdPos(armSubsystem.getShoulderDesState(), armSubsystem.getWristDesState())))
);
Expand Down
19 changes: 11 additions & 8 deletions src/main/java/frc/robot/subsystems/SwerveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ public class SwerveDrive extends SubsystemBase {
SwerveModule FRModule;
SwerveModule BRModule;
WPI_Pigeon2 pigeon;
// private final SwerveDriveOdometry odometer;
private final SwerveDriveOdometry odometer;
private final double maxMetersPerSec, maxMPSAuto;
private final double maxMetersPerSecSquared;
public SwerveDriveKinematics kDriveKinematics;
Expand Down Expand Up @@ -90,7 +90,7 @@ public SwerveDrive(SwerveModule FLModule, SwerveModule BLModule, SwerveModule FR
yController = new PIDController(xykP,xykI,xykD);
kDriveKinematics = new SwerveDriveKinematics(FRModule.moduleTranslation(), FLModule.moduleTranslation(),
BRModule.moduleTranslation(), BLModule.moduleTranslation());
// odometer = new SwerveDriveOdometry(kDriveKinematics, new Rotation2d(0), getModulePositions());
odometer = new SwerveDriveOdometry(kDriveKinematics, new Rotation2d(0), getModulePositions());
align = false;
SmartDashboard.putData("odometry", field);
swerveDrivePoseEstimator = new SwerveDrivePoseEstimator(kDriveKinematics, new Rotation2d(0), getModulePositions(), new Pose2d());
Expand All @@ -103,7 +103,7 @@ public double getDesiredYaw(){

public Command setInitPosition(Pose2d initPose){
return Commands.sequence(Commands.runOnce(()->setPigeon(AllianceFlip.apply(initPose).getRotation().getDegrees())),
// Commands.runOnce(()->odometer.update(getRotation2d(),getModulePositions())),
Commands.runOnce(()->odometer.update(getRotation2d(),getModulePositions())),
Commands.runOnce(() -> {}), //wait a cycle to reset the pigeon or everything breaks
Commands.runOnce(() -> {}),
Commands.runOnce(()->resetOdometry(AllianceFlip.apply(initPose))),
Expand Down Expand Up @@ -134,9 +134,10 @@ public Rotation2d getRotation2d() {
public Pose2d getEstimatedPose(){
return swerveDrivePoseEstimator.getEstimatedPosition();
}
public Pose2d getOdomOnlyPose(){return odometer.getPoseMeters();}

public void resetOdometry(Pose2d pose) {
// odometer.resetPosition(getRotation2d(), getModulePositions(), pose);
odometer.resetPosition(getRotation2d(), getModulePositions(), pose);
swerveDrivePoseEstimator.resetPosition(getRotation2d(), getModulePositions(), pose);
}

Expand All @@ -158,8 +159,8 @@ public void periodic() {
SmartDashboard.putNumber("yaw", getYaw());
SmartDashboard.putNumber("Yaw2d",getRotation2d().getDegrees());
SmartDashboard.putNumber("desired yaw", getDesiredYaw());
// odometer.update(getRotation2d(), getModulePositions());
// field.getObject("odom").setPose(odometer.getPoseMeters());
odometer.update(getRotation2d(), getModulePositions());
field.getObject("odom").setPose(odometer.getPoseMeters());
// vision.setOdometryPosition(odometer.getPoseMeters());
SmartDashboard.putNumber("Rotation",getEstimatedPose().getRotation().getDegrees());
swerveDrivePoseEstimator.update(getRotation2d(), getModulePositions());
Expand Down Expand Up @@ -212,7 +213,8 @@ public Command generateTrajectory(Pose2d start, Pose2d end, ArrayList<Translatio
SwerveControllerCommand trajCommand = new SwerveControllerCommand(
trajectory,
// vision::getRobotPosition,
this::getEstimatedPose,
// this::getEstimatedPose,
this::getOdomOnlyPose,
kDriveKinematics,
xController,
yController,
Expand Down Expand Up @@ -240,7 +242,8 @@ public Command generateTrajectoryQuintic(ArrayList<Pose2d> internalPoints, doubl
SwerveControllerCommand trajCommand = new SwerveControllerCommand(
trajectory,
// vision::getRobotPosition,
this::getEstimatedPose,
// this::getEstimatedPose,
this::getOdomOnlyPose,
kDriveKinematics,
xController,
yController,
Expand Down

0 comments on commit 51f91b0

Please sign in to comment.