Skip to content

Improve structure of command-based framework #22

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

Improve structure of command-based framework #22

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 10 additions & 1 deletion build.gradle
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2023.4.2"
id "edu.wpi.first.GradleRIO" version "2023.4.3"
id "com.diffplug.spotless" version "6.18.0"
}

sourceCompatibility = JavaVersion.VERSION_11
Expand Down Expand Up @@ -97,3 +98,11 @@ wpi.java.configureTestTasks(test)
tasks.withType(JavaCompile) {
options.compilerArgs.add '-XDstringConcat=inline'
}

spotless {
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

./gradlew spotlessApply formats the code consistently.

java {
googleJavaFormat()
// fix formatting of type annotations
formatAnnotations()
}
}
0 ...eploy/pathplanner/leftToLeftBallBLUE.path → ...eploy/pathplanner/leftToLeftBallBlue.path
View file
Open in desktop
File renamed without changes.
0 ...deploy/pathplanner/leftToLeftBallRED.path → ...deploy/pathplanner/leftToLeftBallRed.path
View file
Open in desktop
File renamed without changes.
231 changes: 114 additions & 117 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,134 +15,131 @@
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
@SuppressWarnings("unused")
public final class Constants {

public final static class DrivetrainConstants {
public enum LedColor {
BALANCING(0.57), // PINK
DOCKED(0.77), // GREEN
GRABBER_OPEN(0.67), // GOLD
GRABBER_CLOSED(0.93), // WHITE
BLUE_ALLIANCE(0.87), // BLUE
RED_ALLIANCE(0.61); // RED

public static final double DRIFT_DEADBAND = 0.1;
public static final double ROTATION_DEADBAND = 0.006;
private final double color;

public static final double WIDTH = 22.5;
public static final double LENGTH = 22.5;

public final static int BACK_RIGHT_SPEED_ID = 4;
public final static int BACK_RIGHT_ANGLE_ID = 3;
public final static int BACK_RIGHT_ENCODER = 0;
public final static double BACK_RIGHT_ENCODER_OFFSET = 0.829 - 0.25;

public final static int BACK_LEFT_SPEED_ID = 2;
public final static int BACK_LEFT_ANGLE_ID = 1;
public final static int BACK_LEFT_ENCODER = 1;
public final static double BACK_LEFT_ENCODER_OFFSET = 1-0.512;

public final static int FRONT_RIGHT_SPEED_ID = 6;
public final static int FRONT_RIGHT_ANGLE_ID = 5;
public final static int FRONT_RIGHT_ENCODER = 2;
public final static double FRONT_RIGHT_ENCODER_OFFSET = 0.142;

public final static int FRONT_LEFT_SPEED_ID = 8;
public final static int FRONT_LEFT_ANGLE_ID = 7;
public final static int FRONT_LEFT_ENCODER = 3;
public final static double FRONT_LEFT_ENCODER_OFFSET = 0.082 + 0.25;

public final static Translation2d FRONT_LEFT_LOC = new Translation2d(0.265, 0.265);
public final static Translation2d FRONT_RIGHT_LOC = new Translation2d(0.265, -0.265);
public final static Translation2d BACK_LEFT_LOC = new Translation2d(-0.265, 0.265);
public final static Translation2d BACK_RIGHT_LOC = new Translation2d(-0.265, -0.265);

public final static SwerveDriveKinematics SWERVE_KINEMATICS = new SwerveDriveKinematics(
FRONT_LEFT_LOC,
FRONT_RIGHT_LOC,
BACK_LEFT_LOC,
BACK_RIGHT_LOC);
}

public final static class WheelConstants {
public final static double ROTATIONS_TO_METERS = 0.245/5.08;
}

public final static class ArmConstants {
public final static int PIVOT_MOTOR_ID = 10;
public final static int TELESCOPE_MOTOR_ID = 11;
public final static double pivotOffset = 0.687;

public final static double startingPivotG = 0.8;//2.01;
public final static double pivotGPerTelescopeMeter = 0.8;
public final static double telescopeRotationToMeters = -0.09 / 32768;

public static final int CLICKS_PER_ROTAITON = 2048;

public final static double telescopeOuterSetpoint = 0.9;
public final static double telescopeMiddleSetpoint = 0.3;
public final static double telescopeSubstationSetpoint = 0.568;

public static double elevatorUpPivotDownPosition = 0.81;
public static double pivotDownPosition = 0.83;
LedColor(double color) {
this.color = color;
}

public final static class GrabberConstants {
public final static int PNEUMATICS_HUB_ID = 23;
public final static int LEFT_SOLENOID_REVERSE = 3;
public final static int LEFT_SOLENOID_FORWARD = 2;

public final static int RIGHT_SOLENOID_REVERSE = 1;
public final static int RIGHT_SOLENOID_FORWARD = 0;
}

public final static class ElevatorConstants {
public final static int ELEVATOR_MOTOR_ID = 9;
}

public final static class DriverConstants {
public final static double speedMultiplier = 3;
public final static double angleMultiplier = 3;
}

public final static class AutoConstants {
public final static double autoPivotDown = 0.75;
public final static double autoPivotUp = 0.015;
public final static double pivotDeadband = 0.012;

public final static double autoTelescopeIn = 0.45;
public final static double autoTelescopeWait = 0.6;
public final static double telescopeDeadband = 0.05;
}

public final static class LedConstants {
public final static int BLINKIN_CHANNEL = 0;
public double color() {
return color;
}
}

public enum LedColor {
BALANCING(0.57), //PINK
DOCKED(0.77), //GREEN
GRABBER_OPEN(0.67), //GOLD
GRABBER_CLOSED(0.93), //WHITE
BLUE_ALLIANCE(0.87), //BLUE
RED_ALLIANCE(0.61); //RED

private double color;
public enum LimelightDirections {
GRID_SIDE(180),
SUBSTATION_SIDE(0);

LedColor(double color) {
this.color = color;
}

public double color() {
return color;
}
private final int angle;

LimelightDirections(int angle) {
this.angle = angle;
}

public enum LimelightDirections {
GRID_SIDE(180), SUBSTATION_SIDE(0);

private int angle;
LimelightDirections(int angle){
this.angle = angle;
}

public int angle(){
return angle;
}

public int angle() {
return angle;
}
}

public static final class DrivetrainConstants {

public static final double DRIFT_DEADBAND = 0.1;
public static final double ROTATION_DEADBAND = 0.006;

public static final int BACK_RIGHT_SPEED_ID = 4;
public static final int BACK_RIGHT_ANGLE_ID = 3;
public static final double BACK_RIGHT_ENCODER_OFFSET = 0.829 - 0.25;

public static final int BACK_LEFT_SPEED_ID = 2;
public static final int BACK_LEFT_ANGLE_ID = 1;
public static final double BACK_LEFT_ENCODER_OFFSET = 1 - 0.512;

public static final int FRONT_RIGHT_SPEED_ID = 6;
public static final int FRONT_RIGHT_ANGLE_ID = 5;
public static final double FRONT_RIGHT_ENCODER_OFFSET = 0.142;

public static final int FRONT_LEFT_SPEED_ID = 8;
public static final int FRONT_LEFT_ANGLE_ID = 7;
public static final double FRONT_LEFT_ENCODER_OFFSET = 0.082 + 0.25;

public static final Translation2d FRONT_LEFT_LOC = new Translation2d(0.265, 0.265);
public static final Translation2d FRONT_RIGHT_LOC = new Translation2d(0.265, -0.265);
public static final Translation2d BACK_LEFT_LOC = new Translation2d(-0.265, 0.265);
public static final Translation2d BACK_RIGHT_LOC = new Translation2d(-0.265, -0.265);

public static final SwerveDriveKinematics SWERVE_KINEMATICS =
new SwerveDriveKinematics(FRONT_LEFT_LOC, FRONT_RIGHT_LOC, BACK_LEFT_LOC, BACK_RIGHT_LOC);
}

public static final class WheelConstants {
public static final double ROTATIONS_TO_METERS = 0.245 / 5.08;
}

public static final class ArmConstants {
public static final int PIVOT_MOTOR_ID = 10;
public static final int TELESCOPE_MOTOR_ID = 11;
public static final double pivotOffset = 0.687;

public static final double startingPivotG = 0.8; // 2.01;
public static final double pivotGPerTelescopeMeter = 0.8;
public static final double telescopeRotationToMeters = -0.09 / 32768;

public static final double telescopeOuterSetpoint = 0.9;
public static final double telescopeMiddleSetpoint = 0.3;
public static final double telescopeBottomSetpoint = 0.3;
public static final double telescopeSubstationSetpoint = 0.568;

public static final double elevatorUpPivotDownPosition = 0.81;
public static final double pivotDownPosition = 0.83;

public static final double topConePosition = 0.015;
public static final double middleConePosition = 0.015;
public static final double bottomConePosition = 0.015;
public static final double substationConePosition = 0.015;
}

public static final class GrabberConstants {
public static final int PNEUMATICS_HUB_ID = 23;
public static final int LEFT_SOLENOID_REVERSE = 3;
public static final int LEFT_SOLENOID_FORWARD = 2;

public static final int RIGHT_SOLENOID_REVERSE = 1;
public static final int RIGHT_SOLENOID_FORWARD = 0;
}

public static final class ElevatorConstants {
public static final int ELEVATOR_MOTOR_ID = 9;
}

public static final class DriverConstants {
public static final double speedMultiplier = 3;
public static final double angleMultiplier = 3;
}

public static final class AutoConstants {

public static final double autoTelescopeIn = 0.45;
}

public static final class LedConstants {
public static final int BLINKIN_CHANNEL = 0;
}

public enum ConePosition {
TOP,
MIDDLE,
BOTTOM,
SUBSTATION
}
}
63 changes: 24 additions & 39 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This file just has changes recommend by IDE. You can ignore

Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
Expand All @@ -21,13 +20,11 @@
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;

private RobotContainer m_robotContainer;

NetworkTableInstance nt = NetworkTableInstance.getDefault();
NetworkTable table = nt.getTable("robot");
NetworkTableEntry ntIsEnabled = table.getEntry("isEnabled");
final NetworkTableInstance nt = NetworkTableInstance.getDefault();
final NetworkTable table = nt.getTable("robot");
final NetworkTableEntry ntIsEnabled = table.getEntry("isEnabled");
private Command autonomousCommand;
private RobotContainer robotContainer;

/**
* This function is run when the robot is first started up and should be used for any
Expand All @@ -38,11 +35,11 @@ public void robotInit() {
ntIsEnabled.setBoolean(false);
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer(this);
m_robotContainer.getOdometry().zeroHeading();
m_robotContainer.getOdometry().setAngleOffset(180);
m_robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(180)));
m_robotContainer.getSwerveDriveSubsystem().resetLockRot();
robotContainer = new RobotContainer(this);
robotContainer.getOdometry().zeroHeading();
robotContainer.getOdometry().setAngleOffset(180);
robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(180)));
robotContainer.getSwerveDriveSubsystem().resetLockRot();
}

/**
Expand All @@ -65,36 +62,25 @@ public void robotPeriodic() {
@Override
public void disabledInit() {
ntIsEnabled.setBoolean(false);
m_robotContainer.coastDrive();

// m_robotContainer.getOdometry().zeroHeading();
// m_robotContainer.getOdometry().setAngleOffset(180);
// m_robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(180)));
// m_robotContainer.getSwerveDriveSubsystem().resetLockRot();
robotContainer.coastDrive();
}

@Override
public void disabledPeriodic() {
m_robotContainer.updatePivotTarget();
// m_robotContainer.getOdometry().zeroHeading();
// m_robotContainer.getOdometry().setAngleOffset(180);
// m_robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(180)));
// m_robotContainer.getSwerveDriveSubsystem().resetLockRot();
}
public void disabledPeriodic() {}

/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
ntIsEnabled.setBoolean(true);
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
m_robotContainer.getOdometry().zeroHeading();
m_robotContainer.getOdometry().setAngleOffset(180);
m_robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(180)));
m_robotContainer.getSwerveDriveSubsystem().resetLockRot();
autonomousCommand = robotContainer.getAutonomousCommand();
robotContainer.getOdometry().zeroHeading();
robotContainer.getOdometry().setAngleOffset(180);
robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(180)));
robotContainer.getSwerveDriveSubsystem().resetLockRot();

// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
if (autonomousCommand != null) {
autonomousCommand.schedule();
}
}

Expand All @@ -109,18 +95,17 @@ public void teleopInit() {
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
if (autonomousCommand != null) {
autonomousCommand.cancel();
}
m_robotContainer.getSwerveDriveSubsystem().setFieldCentric(true);
// m_robotContainer.configureObjects();
m_robotContainer.zeroSuperstructure();
robotContainer.getSwerveDriveSubsystem().setFieldCentric(true);
// robotContainer.configureObjects();
}

/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {
m_robotContainer.periodic();
robotContainer.periodic();
// System.out.println("ALLIANCE COLOR: " + DriverStation.getAlliance());
}

Expand Down
Loading