Skip to content

Commit

Permalink
Merge branch 'staging' into zhangal/shooterTesting
Browse files Browse the repository at this point in the history
  • Loading branch information
@mimizh2418
mimizh2418 committed Feb 5, 2024
2 parents 4be0c83 + f942fee commit 5d4b34d
Show file tree
Hide file tree
Showing 59 changed files with 2,447 additions and 193 deletions.
2 changes: 2 additions & 0 deletions .gitattributes
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
src/main/deploy/** linguist-language=JSON linguist-generated=true
**/*.java text=auto
2 changes: 1 addition & 1 deletion .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -162,7 +162,7 @@ bin/
*.iml
*.ipr
*.iws
.idea/

out/

# Fleet
Expand Down
3 changes: 3 additions & 0 deletions .idea/.gitignore
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

10 changes: 10 additions & 0 deletions .idea/codeStyles/Project.xml
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

5 changes: 5 additions & 0 deletions .idea/codeStyles/codeStyleConfig.xml
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

85 changes: 77 additions & 8 deletions src/main/java/org/team1540/robot2024/Constants.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
package org.team1540.robot2024;

import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.util.Units;

/**
Expand All @@ -16,7 +18,7 @@ public final class Constants {
// Whether to pull PID constants from SmartDashboard
public static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP
private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY

public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode;

public enum Mode {
Expand All @@ -39,11 +41,14 @@ public enum Mode {
public static final double LOOP_PERIOD_SECS = 0.02;

public static class SwerveConfig {
public static final String CAN_BUS = IS_COMPETITION_ROBOT ? "" : "";
public static final String CAN_BUS = IS_COMPETITION_ROBOT ? "" : "";
public static final int FRONT_LEFT = IS_COMPETITION_ROBOT ? 3 : 0;
public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 4 : 0;
public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 7 : 0;
public static final int BACK_RIGHT = IS_COMPETITION_ROBOT ? 1 : 0;

// TODO: set this id
public static final int PIGEON_ID = 0;
}

public static class Drivetrain {
Expand All @@ -59,6 +64,49 @@ public static class Drivetrain {
public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
}

public static class Indexer {
// TODO: fix these constants
public static final int INTAKE_ID = 11;
public static final int FEEDER_ID = 12;
public static final double FEEDER_KP = 0.5;
public static final double FEEDER_KI = 0.1;
public static final double FEEDER_KD = 0.001;
public static final double FEEDER_KS = 0.0;
public static final double FEEDER_KV = 0.0;
public static final double FEEDER_GEAR_RATIO = 1.0;
public static final double INTAKE_GEAR_RATIO = 1.0;
public static final double INTAKE_MOI = 0.025;
public static final double FEEDER_MOI = 0.025;
public static final int BEAM_BREAK_ID = 0;


}


public static class Vision {
public static final String FRONT_CAMERA_NAME = "limelight-front";
public static final String REAR_CAMERA_NAME = "limelight-rear";

// TODO: measure these offsets
public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0, 0, 0.5, new Rotation3d());
public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0, 0, 0.5, new Rotation3d(0, 0, Math.PI));

// TODO: find these values
public static final double MAX_VISION_DELAY_SECS = 0.08;
public static final double MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC = 1.0;
public static final double MAX_ACCEPTED_LINEAR_SPEED_MPS = 4.0;
public static final double MIN_ACCEPTED_NUM_TAGS = 1;
public static final double MAX_ACCEPTED_AVG_TAG_DIST_METERS = 8.0;
public static final double MAX_ACCEPTED_ELEVATOR_SPEED_MPS = 0.05;

public static final int SIM_RES_WIDTH = 1280;
public static final int SIM_RES_HEIGHT = 960;
public static final Rotation2d SIM_DIAGONAL_FOV = Rotation2d.fromDegrees(100);
public static final double SIM_FPS = 14.5;
public static final double SIM_AVG_LATENCY_MS = 67.0;
}


public static class Shooter {
public static class Flywheels {
// TODO: determine ids
Expand Down Expand Up @@ -114,11 +162,30 @@ public static class Pivot {

public static class Elevator {
public static final double CHAIN_HEIGHT_METERS = Units.inchesToMeters(28.25);
public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(48.0);
public static final double ELEVATOR_MINIMUM_HEIGHT = Units.inchesToMeters(27.0);
public static final double ELEVATOR_MINIMUM_HEIGHT = Units.inchesToMeters(6.0);
public static final double CLIMBING_HOOKS_MINIMUM_HEIGHT = Units.inchesToMeters(12.0);
public static final double ELEVATOR_MAX_HEIGHT = ELEVATOR_MINIMUM_HEIGHT + Units.inchesToMeters(21.0); //TODO: Fix these constants to be more accurate
public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + ELEVATOR_MAX_HEIGHT - ELEVATOR_MINIMUM_HEIGHT;

public static final double GEAR_RATIO = 2.0; //TODO: Get constants right sometime
public static final int LEADER_ID = -1;
public static final int FOLLOWER_ID = -1;
public static final double KS = 0.25;
public static final double KV = 0.12;
public static final double KA = 0.01;
public static final double KP = 4.8;
public static final double KI = 0;
public static final double KD = 0.1;
public static final double KG = 0;
public static final double CRUISE_VELOCITY_MPS = 2;
public static final double MAXIMUM_ACCELERATION_MPS2 = 20;
public static final double JERK_MPS3 = 40;
public static final double SPROCKET_RADIUS_M = 0.022;
public static final double SPROCKET_CIRCUMFERENCE_M = 2 * SPROCKET_RADIUS_M * Math.PI;
public static final double MOTOR_ROTS_TO_METERS = GEAR_RATIO * SPROCKET_CIRCUMFERENCE_M;
public static final double POS_ERR_TOLERANCE_METERS = 0.03;
public static final double SIM_CARRIAGE_MASS_KG = 1.55; //TODO: check this number :)

public enum ElevatorState {
/**
* At max height :D
Expand All @@ -131,27 +198,29 @@ public enum ElevatorState {
/**
* At height for top of initial climb :D
*/
CLIMB(254.0), //TODO: Find these values :D
CLIMB(CHAIN_HEIGHT_METERS + 0.1 - (CLIMBING_HOOKS_MINIMUM_HEIGHT - ELEVATOR_MINIMUM_HEIGHT)), //TODO: Find these values :D
/**
* At height for trap doing :D
*/
TRAP(254.0), //TODO: Find these values :D
TRAP(27.0), //TODO: Find these values :D
/**
* At height for top of initial climb :D
*/
AMP(254.0); //TODO: Find these values :D
AMP(27.0); //TODO: Find these values :D

public final double heightMeters;

ElevatorState(double heightMeters) {
this.heightMeters = heightMeters;
}
}
}

public static class Tramp {
public static final double GEAR_RATIO = 3.0 / 1.0;
public static final double GEAR_RATIO = 3.0;
public static final double TRAP_SCORING_TIME_SECONDS = 1.114; //TODO: Find these values :D
public static final int TRAMP_MOTOR_ID = -1; //TODO: Configure this later
public static final int TRAMP_BEAM_BREAK_CHANNEL = -1; //TODO: Configure this later

}
}
1 change: 1 addition & 0 deletions src/main/java/org/team1540/robot2024/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,7 @@ public void robotPeriodic() {
// This must be called from the robot's periodic block in order for anything in
// the Command-based framework to work.
CommandScheduler.getInstance().run();
if (Constants.currentMode == Constants.Mode.REAL) robotContainer.odometrySignalRefresher.periodic();

// Update mechanism visualiser in sim
if (Robot.isSimulation()) MechanismVisualiser.periodic();
Expand Down
85 changes: 73 additions & 12 deletions src/main/java/org/team1540/robot2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,18 +8,37 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;

import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
import org.team1540.robot2024.Constants.Elevator.ElevatorState;
import org.team1540.robot2024.commands.FeedForwardCharacterization;
import org.team1540.robot2024.commands.SwerveDriveCommand;
import org.team1540.robot2024.commands.elevator.ElevatorManualCommand;
import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
import org.team1540.robot2024.commands.indexer.IntakeCommand;
import org.team1540.robot2024.commands.shooter.ShootSequence;
import org.team1540.robot2024.commands.shooter.TuneShooterCommand;
import org.team1540.robot2024.subsystems.drive.*;
import org.team1540.robot2024.subsystems.elevator.Elevator;
import org.team1540.robot2024.subsystems.elevator.ElevatorIO;
import org.team1540.robot2024.subsystems.elevator.ElevatorIOSim;
import org.team1540.robot2024.subsystems.elevator.ElevatorIOTalonFX;
import org.team1540.robot2024.subsystems.indexer.Indexer;
import org.team1540.robot2024.subsystems.indexer.IndexerIO;
import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax;
import org.team1540.robot2024.subsystems.shooter.*;
import org.team1540.robot2024.subsystems.tramp.Tramp;
import org.team1540.robot2024.subsystems.tramp.TrampIO;
import org.team1540.robot2024.subsystems.tramp.TrampIOSim;
import org.team1540.robot2024.subsystems.tramp.TrampIOSparkMax;
import org.team1540.robot2024.subsystems.shooter.*;
import org.team1540.robot2024.subsystems.vision.AprilTagVision;
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIO;
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOLimelight;
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOSim;
import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
import org.team1540.robot2024.util.swerve.SwerveFactory;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
import org.team1540.robot2024.util.vision.VisionPoseAcceptor;

import static org.team1540.robot2024.Constants.SwerveConfig;

Expand All @@ -34,6 +53,9 @@ public class RobotContainer {
public final Drivetrain drivetrain;
public final Tramp tramp;
public final Shooter shooter;
public final Elevator elevator;
public final Indexer indexer;
public final AprilTagVision aprilTagVision;

// Controller
public final CommandXboxController driver = new CommandXboxController(0);
Expand All @@ -42,9 +64,9 @@ public class RobotContainer {
// Dashboard inputs
public final LoggedDashboardChooser<Command> autoChooser;

public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);

// TODO: testing dashboard inputs, remove for comp
public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000);
public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000);

/**
* The container for the robot. Contains subsystems, OI devices, and commands.
Expand All @@ -55,13 +77,24 @@ public RobotContainer() {
// Real robot, instantiate hardware IO implementations
drivetrain =
new Drivetrain(
new GyroIONavx(),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT)),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT)),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT)));
new GyroIOPigeon2(odometrySignalRefresher),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT), odometrySignalRefresher),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT), odometrySignalRefresher),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
tramp = new Tramp(new TrampIOSparkMax());
shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
elevator = new Elevator(new ElevatorIOTalonFX());
indexer =
new Indexer(
new IndexerIOSparkMax()
);
aprilTagVision = new AprilTagVision(
new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
drivetrain::addVisionMeasurement,
() -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
break;
case SIM:
// Sim robot, instantiate physics sim IO implementations
Expand All @@ -74,8 +107,19 @@ public RobotContainer() {
new ModuleIOSim());
tramp = new Tramp(new TrampIOSim());
shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim());
elevator = new Elevator(new ElevatorIOSim());
aprilTagVision =
new AprilTagVision(
new AprilTagVisionIOSim(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE, drivetrain::getPose),
new AprilTagVisionIOSim(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE, drivetrain::getPose),
ignored -> {},
() -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
indexer =
new Indexer(
new IndexerIOSim()
);
break;

default:
// Replayed robot, disable IO implementations
drivetrain =
Expand All @@ -86,7 +130,19 @@ public RobotContainer() {
new ModuleIO() {},
new ModuleIO() {});
shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {});
elevator = new Elevator(new ElevatorIO() {});
aprilTagVision =
new AprilTagVision(
new AprilTagVisionIO() {},
new AprilTagVisionIO() {},
(ignored) -> {},
() -> 0.0,
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)
);

indexer = new Indexer(new IndexerIO() {});
tramp = new Tramp(new TrampIO() {});

break;
}

Expand Down Expand Up @@ -116,6 +172,9 @@ public RobotContainer() {
*/
private void configureButtonBindings() {
drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver));
elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot));
copilot.rightBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.TOP));
copilot.leftBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM));
driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain));
driver.b().onTrue(
Commands.runOnce(
Expand All @@ -124,8 +183,10 @@ private void configureButtonBindings() {
).ignoringDisable(true)
);

copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get()))
copilot.a().onTrue(new ShootSequence(shooter, indexer))
.onFalse(Commands.runOnce(shooter::stopFlywheels, shooter));

driver.a().onTrue(new IntakeCommand(indexer));
}

/**
Expand Down
10 changes: 5 additions & 5 deletions src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ public void add(double velocity, double voltage) {
}

public void print() {
if (velocityData.size() == 0 || voltageData.size() == 0) {
if (velocityData.isEmpty() || voltageData.isEmpty()) {
return;
}

Expand All @@ -87,10 +87,10 @@ public void print() {
1);

System.out.println("FF Characterization Results:");
System.out.println("\tCount=" + Integer.toString(velocityData.size()) + "");
System.out.println(String.format("\tR2=%.5f", regression.R2()));
System.out.println(String.format("\tkS=%.5f", regression.beta(0)));
System.out.println(String.format("\tkV=%.5f", regression.beta(1)));
System.out.println("\tCount=" + velocityData.size());
System.out.printf("\tR2=%.5f%n", regression.R2());
System.out.printf("\tkS=%.5f%n", regression.beta(0));
System.out.printf("\tkV=%.5f%n", regression.beta(1));
}
}
}
8 changes: 4 additions & 4 deletions src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,9 @@ public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controlle

@Override
public void initialize() {
xLimiter.reset(0);
yLimiter.reset(0);
rotLimiter.reset(0);
xLimiter.reset(0);
yLimiter.reset(0);
rotLimiter.reset(0);
}

@Override
Expand All @@ -46,7 +46,7 @@ public void execute() {
// Calculate new linear velocity
Translation2d linearVelocity =
new Pose2d(new Translation2d(), linearDirection)
.transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();
.transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();

// Convert to field relative speeds & send command
drivetrain.runVelocity(
Expand Down
Loading

0 comments on commit 5d4b34d

Please sign in to comment.