Skip to content

Commit

Permalink
Elastic and Field Helper class
Browse files Browse the repository at this point in the history
  • Loading branch information
Maia Fiur committed Jul 10, 2024
1 parent f0c75b4 commit 630899d
Show file tree
Hide file tree
Showing 9 changed files with 121 additions and 123,946 deletions.
123,876 changes: 0 additions & 123,876 deletions .ignoreme
View file

This file was deleted.

28 changes: 11 additions & 17 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
import frc.robot.Constants.ShootingConstants.ShootingPosition;
import frc.robot.Constants.SwerveConstants;
import frc.robot.subsystems.LED;
import frc.robot.util.FieldHelper;
import frc.robot.subsystems.Head;
import frc.robot.shuffleboard.ArmTab;

Expand Down Expand Up @@ -168,11 +169,11 @@ private void configureDriverBindings(){
.onFalse(Commands.runOnce(() -> speedMultiplier -= SwerveConstants.FAST_SPEED_INCREMENT));

driverControllerCommands.povLeft()
.and(() -> checkAllianceColors(Alliance.Red))
.and(() -> FieldHelper.checkAllianceColors(Alliance.Red))
.whileTrue(drivetrain.driveCommand(() -> processJoystickVelocity(driverControllerCommands.getLeftY()), () -> processJoystickVelocity(driverControllerCommands.getLeftX()), () -> Math.cos(Units.degreesToRadians(-150)), () -> Math.sin(Units.degreesToRadians(-150))));

driverControllerCommands.povLeft()
.and(() -> checkAllianceColors(Alliance.Blue))
.and(() -> FieldHelper.checkAllianceColors(Alliance.Blue))
.whileTrue(drivetrain.driveCommand(() -> processJoystickVelocity(driverControllerCommands.getLeftY()), () -> processJoystickVelocity(driverControllerCommands.getLeftX()), () -> Math.cos(Units.degreesToRadians(150)), () -> Math.sin(Units.degreesToRadians(150))));

driverControllerCommands.povUp().onTrue(Commands.runOnce(() -> speedMultiplier = Math.min(1, speedMultiplier + SwerveConstants.PRECISE_INCREMENT)));
Expand Down Expand Up @@ -241,23 +242,16 @@ public String outputValues(Supplier<Double> distance, Supplier<Double> armAngle)
return "distance: " + distance.get() + "\n arm angle: " + armAngle.get();
}

private boolean checkAllianceColors(Alliance checkAgainst) {
if (DriverStation.getAlliance().isPresent()) {
return DriverStation.getAlliance().get() == checkAgainst;
}
return false;
}

private double processJoystickVelocity(double joystickInput) {
return checkAllianceColors(Alliance.Blue) ? (-MathUtil.applyDeadband(joystickInput, OperatorConstants.DRIVER_JOYSTICK_DEADBAND)) * speedMultiplier : MathUtil.applyDeadband(joystickInput, OperatorConstants.DRIVER_JOYSTICK_DEADBAND) * speedMultiplier;
return FieldHelper.checkAllianceColors(Alliance.Blue) ? (-MathUtil.applyDeadband(joystickInput, OperatorConstants.DRIVER_JOYSTICK_DEADBAND)) * speedMultiplier : MathUtil.applyDeadband(joystickInput, OperatorConstants.DRIVER_JOYSTICK_DEADBAND) * speedMultiplier;
}

private double processJoystickAngular(double joystickInput) {
return checkAllianceColors(Alliance.Blue) ? Math.pow(-MathUtil.applyDeadband(joystickInput, OperatorConstants.DRIVER_JOYSTICK_DEADBAND), 3) : Math.pow(MathUtil.applyDeadband(joystickInput, OperatorConstants.DRIVER_JOYSTICK_DEADBAND), 3);
return FieldHelper.checkAllianceColors(Alliance.Blue) ? Math.pow(-MathUtil.applyDeadband(joystickInput, OperatorConstants.DRIVER_JOYSTICK_DEADBAND), 3) : Math.pow(MathUtil.applyDeadband(joystickInput, OperatorConstants.DRIVER_JOYSTICK_DEADBAND), 3);
}

private double processJoystickAngularButFree(double joystickInput) {
return checkAllianceColors(Alliance.Blue) ? Math.pow(-MathUtil.applyDeadband(joystickInput, OperatorConstants.DRIVER_JOYSTICK_DEADBAND), 3) : Math.pow(-MathUtil.applyDeadband(joystickInput, OperatorConstants.DRIVER_JOYSTICK_DEADBAND), 3);
return FieldHelper.checkAllianceColors(Alliance.Blue) ? Math.pow(-MathUtil.applyDeadband(joystickInput, OperatorConstants.DRIVER_JOYSTICK_DEADBAND), 3) : Math.pow(-MathUtil.applyDeadband(joystickInput, OperatorConstants.DRIVER_JOYSTICK_DEADBAND), 3);
}

public void doVisionUpdates(boolean doVisionUpdates){
Expand All @@ -280,14 +274,14 @@ public void setMotorBrake(boolean isBraked) {
}

public Command turnToSpeaker() {
if (checkAllianceColors(Alliance.Red)) {
if (FieldHelper.checkAllianceColors(Alliance.Red)) {
return new ParallelRaceGroup(new TurnToTag(drivetrain, 4, true), Commands.waitSeconds(1));
}
return new ParallelRaceGroup(new TurnToTag(drivetrain, 7, true), Commands.waitSeconds(1));
}

public Command turnToSpeaker(Supplier<Double> yMovement, Supplier<Double> xMovement) {
if (checkAllianceColors(Alliance.Red)) {
if (FieldHelper.checkAllianceColors(Alliance.Red)) {
return new TurnToTag(drivetrain, 4, true, yMovement, xMovement);
}
return new TurnToTag(drivetrain, 7, true, yMovement, xMovement);
Expand All @@ -301,21 +295,21 @@ public void teleopInit() {
* DOES NOT ACTAULLY TURN TO ZERO BE AWARE
*/
public Command pointAwayFromSpeaker() {
if (checkAllianceColors(Alliance.Red)) {
if (FieldHelper.checkAllianceColors(Alliance.Red)) {
return new ParallelRaceGroup(drivetrain.turnToAngleCommand(Rotation2d.fromDegrees(180)),Commands.waitSeconds(0.5));
}
return new ParallelRaceGroup(drivetrain.turnToAngleCommand(Rotation2d.fromDegrees(0)),Commands.waitSeconds(1));
}

public Command turnSideways(){
if (checkAllianceColors(Alliance.Red)) {
if (FieldHelper.checkAllianceColors(Alliance.Red)) {
return drivetrain.turnToAngleCommand(Rotation2d.fromDegrees(-90));
}
return drivetrain.turnToAngleCommand(Rotation2d.fromDegrees(90));
}

public Command turnAwayFromAmp(){
if (checkAllianceColors(Alliance.Red)) {
if (FieldHelper.checkAllianceColors(Alliance.Red)) {
return drivetrain.turnToAngleCommand(Rotation2d.fromDegrees(90));
}
return drivetrain.turnToAngleCommand(Rotation2d.fromDegrees(-90));
Expand Down
17 changes: 10 additions & 7 deletions src/main/java/frc/robot/shuffleboard/ArmTab.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ public ArmTab(Arm arm) {

NetworkTableInstance inst = NetworkTableInstance.getDefault();

NetworkTable networkTable = inst.getTable("logging/arm");
NetworkTable networkTable = inst.getTable("Shuffleboard/arm");

armAbsoluteEncoderPub = networkTable.getDoubleTopic("Arm Absolute Encoder").publish();
elevatorPub = networkTable.getDoubleTopic("Elevator").publish();
Expand All @@ -39,15 +39,18 @@ public void activateShuffleboard() {
// tab.add("add elevator feed foward values", arm.addElevatorFeedFowardValuesCommand());
// tab.add("generate new arm feed foward values", arm.generateNewArmFeedFoward());
// tab.add("generate new elevator feed foward values", arm.generateNewElevatorFeedFoward());
tab.add("arm subsystem", arm);
tab.add("toggle brake modes", arm.ToggleBrakeModes());
tab.add("extend elevator", arm.RaiseElevator());
tab.add("retract elevator", arm.lowerElevator());
tab.add("stow", arm.Stow());
tab.add("arm subsystem", arm).withPosition(0, 0);
tab.add("toggle brake modes", arm.ToggleBrakeModes()).withPosition(2, 0);
tab.add("extend elevator", arm.RaiseElevator()).withPosition(2, 1);
tab.add("retract elevator", arm.lowerElevator()).withPosition(2, 2);
tab.add("stow", arm.Stow()).withPosition(2, 3);

tab.add("Arm Absolute Encoder", 0.0).withPosition(0, 1);
tab.add("Elevator", 0.0).withPosition(1, 1);
// tab.add("stop arm", new InstantCommand(() -> arm.stopArm()).ignoringDisable(true));
// tab.add("foward run SysidQuasistatic", arm.SysidQuasistatic(Direction.kForward));
// tab.add("backward run SysidQuasistatic", arm.SysidQuasistatic(Direction.kReverse));
arm.getMotorTab().activateShuffleboard();
//arm.getMotorTab().activateShuffleboard();
}

@Override
Expand Down
11 changes: 9 additions & 2 deletions src/main/java/frc/robot/shuffleboard/ClimberTab.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import frc.robot.subsystems.Climber;

public class ClimberTab extends ShuffleboardTabBase {
Expand All @@ -19,7 +21,7 @@ public ClimberTab(Climber climber) {

NetworkTableInstance inst = NetworkTableInstance.getDefault();

NetworkTable networkTable = inst.getTable("logging/climber");
NetworkTable networkTable = inst.getTable("Shuffleboard/climber");

leftMotorPosition = networkTable.getDoubleTopic("Left Motor Position").publish();

Expand All @@ -29,7 +31,7 @@ public ClimberTab(Climber climber) {

rightMotorSpeed = networkTable.getDoubleTopic("Right Motor Speed").publish();

motorTab.addMotor(climber.getMotors());
//motorTab.addMotor(climber.getMotors());


}
Expand All @@ -45,6 +47,11 @@ public void update() {

@Override
public void activateShuffleboard() {
ShuffleboardTab tab = Shuffleboard.getTab("climber");
tab.add("Left Motor Position", 0.0).withPosition(0, 0);
tab.add("Left Motor Speed", 0.0).withPosition(0, 1);
tab.add("Right Motor Position", 0.0).withPosition(1, 0);
tab.add("Right Motor Speed", 0.0).withPosition(1, 1);
}

@Override
Expand Down
12 changes: 4 additions & 8 deletions src/main/java/frc/robot/shuffleboard/DriverStationTab.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,14 +50,6 @@ public DriverStationTab(SendableChooser<Command> autoChooser, DigitalInput butto
isBrownedOut = networkTable.getBooleanTopic("Is Browned Out").publish();
this.button = button;
isButtonPushed = networkTable.getBooleanTopic("is brake button pushed").publish();

// camera = CameraServer.startAutomaticCapture();
/*
* if (camera.isConnected()) {
* camera.setResolution(480, 320);
* camera.setFPS(10);
* }
*/
}

@Override
Expand All @@ -73,6 +65,10 @@ public void activateShuffleboard() {
// this should be called immediately
ShuffleboardTab tab = Shuffleboard.getTab("Driver Station");
tab.add("Auto Path", autoChooser);
tab.add("Battery Voltage", 0.0).withPosition(4, 1);
tab.add("Is Driver Station Connected", false).withPosition(4, 0);
tab.add("Is Browned Out", false).withPosition(4, 2);
tab.add("is brake button pushed", false).withPosition(4, 3);
}

@Override
Expand Down
48 changes: 21 additions & 27 deletions src/main/java/frc/robot/shuffleboard/HeadTab.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,25 +35,21 @@ public HeadTab(Head head) {

NetworkTableInstance inst = NetworkTableInstance.getDefault();

NetworkTable networkTable = inst.getTable("logging/Head");
NetworkTable networkTable = inst.getTable("Shuffleboard/Head");

NetworkTable intakeNetworkTable = inst.getTable("logging/Head/Intake");
noteWithinSensorPublisher = intakeNetworkTable.getBooleanTopic("Note Within Sensor").publish();
noteAcquiredPublisher = intakeNetworkTable.getBooleanTopic("Note Acquired").publish();
noteWithinSensorPublisher = networkTable.getBooleanTopic("Note Within Sensor").publish();
noteAcquiredPublisher = networkTable.getBooleanTopic("Note Acquired").publish();

NetworkTable shooterNetworkTable = inst.getTable("logging/Head/Shooter");
shooterBottomSpeedPublisher = shooterNetworkTable.getDoubleTopic("Bottom Speed").publish();
shooterTopSpeedPublisher = shooterNetworkTable.getDoubleTopic("Top Speed").publish();
shooterSetPointPublisher = shooterNetworkTable.getDoubleTopic("Setpoint").publish();
shooterBottomSpeedPublisher = networkTable.getDoubleTopic("Bottom Speed").publish();
shooterTopSpeedPublisher = networkTable.getDoubleTopic("Top Speed").publish();
shooterSetPointPublisher = networkTable.getDoubleTopic("Setpoint").publish();

readyToShootPublisher = shooterNetworkTable.getBooleanTopic("Ready to Shoot").publish();
readyToShootPublisher = networkTable.getBooleanTopic("Ready to Shoot").publish();
}

@Override
public void update() {
noteWithinSensorPublisher.set(head.isNoteWithinSensor());
// noteInShooterPublisher.set(head.isNoteInShooter());

noteWithinSensorPublisher.set(head.isNoteWithinSensor());
shooterBottomSpeedPublisher.set(head.getShooterBottomSpeed());
shooterTopSpeedPublisher.set(head.getShooterTopSpeed());
shooterSetPointPublisher.set(head.getShooterSetPoint());
Expand All @@ -65,28 +61,26 @@ public void update() {
public void activateShuffleboard() {
ShuffleboardTab tab = Shuffleboard.getTab("Head");
// Intake
ShuffleboardLayout intakeLayout = tab.getLayout("Intake", BuiltInLayouts.kGrid).withSize(5, 2).withPosition(0, 0);
intakeLayout.add("Note Within Sensor", false).withPosition(0, 0);
tab.add("Note Within Sensor", false).withPosition(0, 0);

intakeLayout.add("Intake", head.IntakePiece()).withPosition(1, 0);
intakeLayout.add("Manual Intake", head.StartIntake()).withPosition(2, 0);
intakeLayout.add("Outake", head.OutakePiece()).withPosition(3, 0);
intakeLayout.add("Manual Outake", head.StartOutake()).withPosition(3, 1);
tab.add("Intake", head.IntakePiece()).withPosition(0, 1);
tab.add("Manual Intake", head.StartIntake()).withPosition(0, 2);
tab.add("Outake", head.OutakePiece()).withPosition(2, 1);
tab.add("Manual Outake", head.StartOutake()).withPosition(2, 2);

intakeLayout.add("Stop Intake", head.StopIntake()).withPosition(4, 0);
tab.add("Stop Intake", head.StopIntake()).withPosition(2, 0);

intakeLayout.add("Feeder Speed", 0.0).withPosition(1, 2);
tab.add("Feeder Speed", 0.0).withPosition(5, 2);

// Shooter
ShuffleboardLayout shooterLayout = tab.getLayout("Shooter", BuiltInLayouts.kGrid).withSize(5, 2).withPosition(0, 2);
shooterLayout.add("Top Speed", 0.0).withPosition(0, 0);
shooterLayout.add("Bottom Speed", 0.0).withPosition(0, 1);
shooterLayout.add("Setpoint", 0.0).withPosition(0, 2);
tab.add("Top Speed", 0.0).withPosition(4, 0);
tab.add("Bottom Speed", 0.0).withPosition(4, 2);
tab.add("Setpoint", 0.0).withPosition(4, 1);

shooterLayout.add("Ready to Shoot", false).withPosition(1, 0);
tab.add("Ready to Shoot", false).withPosition(1, 0);

shooterLayout.add("Spin Up (speaker)", head.SpinUpShooter(ShootingConstants.ShootingPosition.SUBWOOFER)).withPosition(2, 0);
shooterLayout.add("Spin Down", head.SpinDownShooter()).withPosition(2, 1);
tab.add("Spin Up (speaker)", head.SpinUpShooter(ShootingConstants.ShootingPosition.SUBWOOFER)).withPosition(5, 0);
tab.add("Spin Down", head.SpinDownShooter()).withPosition(5, 1);
//shooterLayout.add("Shoot (speaker)", head.Shoot(ShootingConstants.ShootingPosition.SUBWOOFER)).withPosition(2, 2);
}

Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/shuffleboard/ShuffleboardInfo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ public void activateTabs() {
@Override
public void periodic() {
// if robot is not connected to the field system, enable shuffleboard
if (false) {
if (true) {
activateTabs();
}
// This method will be called once per scheduler run
Expand Down
17 changes: 9 additions & 8 deletions src/main/java/frc/robot/shuffleboard/SwerveTab.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@
import edu.wpi.first.networktables.IntegerPublisher;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import frc.robot.subsystems.Drivetrain;

public class SwerveTab extends ShuffleboardTabBase {
Expand All @@ -13,8 +15,6 @@ public class SwerveTab extends ShuffleboardTabBase {
private final DoublePublisher odometryXPub;
private final DoublePublisher odometryAnglePub;
private final DoubleArrayPublisher posePublisher;
private int number = 0;
private final IntegerPublisher numPublisher;

public SwerveTab(Drivetrain swerveDrive) {
this.swerveDrive = swerveDrive;
Expand All @@ -29,19 +29,14 @@ public SwerveTab(Drivetrain swerveDrive) {

odometryAnglePub = networkTable.getDoubleTopic("Angle Odometry").publish();

numPublisher = networkTable.getIntegerTopic("number").publish();

posePublisher = networkTable.getDoubleArrayTopic("Pose").publish();
}

@Override
public void update() {
// these functions have not been defined
odometryAnglePub.set(swerveDrive.getPose().getRotation().getDegrees());
odometryXPub.set(swerveDrive.getPose().getX());
odometryYPub.set(swerveDrive.getPose().getY());
number += 1;
numPublisher.set(number);
double[] pose = new double[3];
pose[0] = swerveDrive.getPose().getX();
pose[1] = swerveDrive.getPose().getY();
Expand All @@ -51,7 +46,13 @@ public void update() {

@Override
public void activateShuffleboard() {
// shuffleboardTab.add(this.swerveDrive);
ShuffleboardTab tab = Shuffleboard.getTab("swerveDrive");
tab.add("X Odometry", 0.0).withPosition(0, 0);
tab.add("Y Odometry", 0.0).withPosition(1, 0);
tab.add("Angle Odometry", 0.0).withPosition(0, 1);
tab.add("pose", new double[3]).withPosition(0, 3);


}

@Override
Expand Down
56 changes: 56 additions & 0 deletions src/main/java/frc/robot/util/FieldHelper.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,56 @@
package frc.robot.util;

import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.hal.AllianceStationID;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;

import java.io.IOException;
import java.lang.Exception;
import java.util.Optional;

public class FieldHelper {
private static AprilTagFieldLayout fieldLayout;

enum TagLocations {
SOURCE_CLOSE(1, 10), SOURCE_FAR(2, 9), SPEAKER(7, 4), AMP(6, 5);

private final int blueId;
private final int redId;

TagLocations(int blueId, int redId) {
this.blueId = blueId;
this.redId = redId;
}

public Optional<Pose3d> getPose() {
if (fieldLayout == null) {
try {
fieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile);
} catch (Exception e) {
fieldLayout = null;
return Optional.empty();
}
}
try{
if(DriverStation.getAlliance().orElseThrow() == Alliance.Blue){
return fieldLayout.getTagPose(blueId);
}
else{
return fieldLayout.getTagPose(redId);
}}
catch(Exception e){
return Optional.empty();
}
}
}

public static boolean checkAllianceColors(Alliance checkAgainst) {
if (DriverStation.getAlliance().isPresent()) {
return DriverStation.getAlliance().orElseThrow() == checkAgainst;
}
return false;
}
}

0 comments on commit 630899d

Please sign in to comment.