Skip to content

Commit

Permalink
Changed pid values, commented out other sub systems in robot containe…
Browse files Browse the repository at this point in the history 
…r, drivetrain added a time elapsed variable for debug
  • Loading branch information
@DriverStationComputer
DriverStationComputer committed Oct 23, 2024
1 parent 0cf5a43 commit 6fbf25f
Show file tree
Hide file tree
Showing 3 changed files with 142 additions and 127 deletions.
36 changes: 23 additions & 13 deletions src/main/java/org/carlmontrobotics/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -254,7 +254,8 @@ public static final class Drivetrainc {
// front-left, front-right, back-left, back-right.
// Determine correct turn PID constants
public static final double[] turnkP = CONFIG.isSwimShady()
? new double[] {0.00374, 0.00374, 0.00374, 0.00374}
? new double[] {0.00374 * 350, 0.00374 * 350, 0.00374 * 350,
0.00374 * 350}
: new double[] {51.078, 60.885, 60.946, 60.986};
public static final double[] turnkI = { 0, 0, 0, 0 };
public static final double[] turnkD =
Expand All @@ -280,7 +281,8 @@ public static final class Drivetrainc {
// Forward: 1.72, 1.71, 1.92, 1.94
// Backward: 1.92, 1.92, 2.11, 1.89
// Order of modules: (FL, FR, BL, BR)
public static final double[] drivekP = CONFIG.isSwimShady() ? new double[] { 2.8, 2.8, 2.8, 2.8 }
public static final double[] drivekP = CONFIG.isSwimShady()
? new double[] {2.8 / 6, 2.8 / 6, 2.8 / 6, 2.8 / 6}
: new double[] { 1.75, 1.75, 1.75, .75 }; // {1.82/100, 1.815/100, 2.015/100,
// 1.915/100};
public static final double[] drivekI = { 0, 0, 0, 0 };
Expand Down Expand Up @@ -343,19 +345,27 @@ public static final class Drivetrainc {

// #region Ports

public static final int driveFrontLeftPort = CONFIG.isSwimShady() ? 18 : 11; //
public static final int driveFrontRightPort = CONFIG.isSwimShady() ? 10 : 19; //
public static final int driveBackLeftPort = CONFIG.isSwimShady() ? 19 : 14; //
public static final int driveBackRightPort = CONFIG.isSwimShady() ? 1 : 17; // correct

public static final int turnFrontLeftPort = CONFIG.isSwimShady() ? 17 : 12; //
public static final int turnFrontRightPort = CONFIG.isSwimShady() ? 9 : 20; // 20
public static final int turnBackLeftPort = CONFIG.isSwimShady() ? 20 : 15; //
public static final int turnBackRightPort = CONFIG.isSwimShady() ? 2 : 16; // correct
public static final int driveFrontLeftPort =
CONFIG.isSwimShady() ? 1 : 11; //
public static final int driveFrontRightPort =
CONFIG.isSwimShady() ? 2 : 19; //
public static final int driveBackLeftPort =
CONFIG.isSwimShady() ? 3 : 14; //
public static final int driveBackRightPort =
CONFIG.isSwimShady() ? 4 : 17; // correct

public static final int turnFrontLeftPort =
CONFIG.isSwimShady() ? 11 : 12; //
public static final int turnFrontRightPort =
CONFIG.isSwimShady() ? 12 : 20; // 20
public static final int turnBackLeftPort =
CONFIG.isSwimShady() ? 13 : 15; //
public static final int turnBackRightPort =
CONFIG.isSwimShady() ? 14 : 16; // correct

public static final int canCoderPortFL = CONFIG.isSwimShady() ? 1 : 0;
public static final int canCoderPortFR = CONFIG.isSwimShady() ? 3 : 3;
public static final int canCoderPortBL = CONFIG.isSwimShady() ? 2 : 2;
public static final int canCoderPortFR = CONFIG.isSwimShady() ? 2 : 3;
public static final int canCoderPortBL = CONFIG.isSwimShady() ? 3 : 2;
public static final int canCoderPortBR = CONFIG.isSwimShady() ? 4 : 1;

// #endregion
Expand Down
218 changes: 109 additions & 109 deletions src/main/java/org/carlmontrobotics/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,14 +71,14 @@ public class RobotContainer {
// 1. using GenericHID allows us to use different kinds of controllers
// 2. Use absolute paths from constants to reduce confusion
public final GenericHID driverController = new GenericHID(Driver.port);
public final GenericHID manipulatorController = new GenericHID(Manipulator.port);
private final IntakeShooter intakeShooter = new IntakeShooter();
// public final GenericHID manipulatorController = new GenericHID(Manipulator.port);
// private final IntakeShooter intakeShooter = new IntakeShooter();

// ignore warning, LED must be initialized
private final Led led = new Led(intakeShooter);
private final Arm arm = new Arm();
// private final Led led = new Led(intakeShooter);
// private final Arm arm = new Arm();
private final Drivetrain drivetrain = new Drivetrain();
private final Limelight limelight = new Limelight(drivetrain);
// private final Limelight limelight = new Limelight(drivetrain);

/* These are assumed to be equal to the AUTO ames in pathplanner */
/* These must be equal to the pathPlanner path names from the GUI! */
Expand Down Expand Up @@ -153,17 +153,17 @@ private void setDefaultCommands() {
() -> ProcessedAxisValue(driverController, Axis.kRightX),
() -> driverController.getRawButton(Driver.slowDriveButton)));
// TODO: Are we going to use default command for intakeshooter?
intakeShooter.setDefaultCommand(new TeleopEffector(intakeShooter,
() -> ProcessedAxisValue(manipulatorController, Axis.kLeftY),
manipulatorController, driverController));
// intakeShooter.setDefaultCommand(new TeleopEffector(intakeShooter,
// () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY),
// manipulatorController, driverController));
// TODO
// intakeShooter.setDefaultCommand(new RampMaxRPMDriving(intakeShooter));

arm.setDefaultCommand(
Config.CONFIG.useSmartDashboardControl ? new TestArmToPos(arm)
: new TeleopArm(arm,
() -> ProcessedAxisValue(manipulatorController,
Axis.kLeftY)));
// arm.setDefaultCommand(
// Config.CONFIG.useSmartDashboardControl ? new TestArmToPos(arm)
// : new TeleopArm(arm,
// () -> ProcessedAxisValue(manipulatorController,
// Axis.kLeftY)));

}

Expand All @@ -174,18 +174,18 @@ private void setBindingsDriver() {
// .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"),
// new AutoMATICALLYGetNote(drivetrain, intakeShooter, limelight)));

new POVButton(driverController, 0)
.whileTrue(new ParallelCommandGroup(new Intake(intakeShooter),
new AutoMATICALLYGetNote(drivetrain, limelight,
intakeShooter, 1)));
// new POVButton(driverController, 0)
// .whileTrue(new ParallelCommandGroup(new Intake(intakeShooter),
// new AutoMATICALLYGetNote(drivetrain, limelight,
// intakeShooter, 1)));

axisTrigger(driverController, Axis.kLeftTrigger)
// .onTrue(new AlignToApriltag(drivetrain, limelight));
.onTrue(new InstantCommand(() -> drivetrain.setFieldOriented(false)))
.onFalse(new InstantCommand(() -> drivetrain.setFieldOriented(true)));

axisTrigger(driverController, Manipulator.SHOOTER_BUTTON)
.whileTrue(new AlignToApriltag(drivetrain, limelight, 2.0));
// axisTrigger(driverController, Manipulator.SHOOTER_BUTTON)
// .whileTrue(new AlignToApriltag(drivetrain, limelight, 2.0));
new JoystickButton(driverController, Driver.rotateFieldRelative0Deg).onTrue(
new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(0), drivetrain));
new JoystickButton(driverController, Driver.rotateFieldRelative90Deg)
Expand All @@ -200,30 +200,30 @@ private void setBindingsDriver() {
}

private void setBindingsManipulator() {
new JoystickButton(manipulatorController, EJECT_BUTTON)
.onTrue(new Eject(intakeShooter));
// new JoystickButton(manipulatorController, EJECT_BUTTON)
// .onTrue(new Eject(intakeShooter));

// new JoystickButton(manipulatorController, A_BUTTON)
// .onTrue(new RampMaxRPMDriving(intakeShooter));
// axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue(
// new SequentialCommandGroup(new AimArmSpeaker(arm, limelight),
// new PassToOuttake(intakeShooter)));

axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue(
new ConditionalCommand(new SequentialCommandGroup(new AimArmSpeaker(arm, limelight),
new PassToOuttake(intakeShooter)), new InstantCommand(() -> {
}), () -> LimelightHelpers.getTV(SHOOTER_LL_NAME)));
// axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue(
// new ConditionalCommand(new SequentialCommandGroup(new AimArmSpeaker(arm, limelight),
// new PassToOuttake(intakeShooter)), new InstantCommand(() -> {
// }), () -> LimelightHelpers.getTV(SHOOTER_LL_NAME)));

// axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON)
// .whileTrue(new PassToOuttake(intakeShooter));

// axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON)
// .whileTrue(new AimArmSpeaker(arm, limelight));

new JoystickButton(manipulatorController, RAMP_OUTTAKE)
.whileTrue(new RampMaxRPM(intakeShooter));
new JoystickButton(manipulatorController, OPPOSITE_EJECT)
.whileTrue(new EjectOuttakeSide(intakeShooter));
// new JoystickButton(manipulatorController, RAMP_OUTTAKE)
// .whileTrue(new RampMaxRPM(intakeShooter));
// new JoystickButton(manipulatorController, OPPOSITE_EJECT)
// .whileTrue(new EjectOuttakeSide(intakeShooter));

/*
* axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON)
Expand All @@ -235,23 +235,23 @@ private void setBindingsManipulator() {
// axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON)
// .onTrue(new PassToOuttake(intakeShooter));

axisTrigger(manipulatorController, Manipulator.INTAKE_BUTTON)
.whileTrue(new Intake(intakeShooter));
new JoystickButton(manipulatorController, ARM_TO_AMP_BUTTON)
.onTrue(new ArmToPos(arm, AMP_ANGLE_RAD_NEW_MOTOR));
new JoystickButton(manipulatorController, A_BUTTON)
.onTrue(new ArmToPos(arm, GROUND_INTAKE_POS));
new JoystickButton(manipulatorController, PASS_TO_OUTTAKE_STICK)
.onTrue(new PassToOuttake(intakeShooter));
new JoystickButton(manipulatorController, PASS_TO_INTAKE_STICK)
.onTrue(new PassToIntake(intakeShooter));
new JoystickButton(manipulatorController, SPEAKER_POS)
.onTrue(new ArmToPos(arm, SPEAKER_ANGLE_RAD));
new POVButton(manipulatorController, UP_D_PAD)
.onTrue(new ArmToPos(arm, CLIMB_POS));
new POVButton(manipulatorController, DOWN_D_PAD).onTrue(new Climb(arm));
new POVButton(manipulatorController, LEFT_D_PAD)
.onTrue(new ArmToPos(arm, PODIUM_ANGLE_RAD));
// axisTrigger(manipulatorController, Manipulator.INTAKE_BUTTON)
// .whileTrue(new Intake(intakeShooter));
// new JoystickButton(manipulatorController, ARM_TO_AMP_BUTTON)
// .onTrue(new ArmToPos(arm, AMP_ANGLE_RAD_NEW_MOTOR));
// new JoystickButton(manipulatorController, A_BUTTON)
// .onTrue(new ArmToPos(arm, GROUND_INTAKE_POS));
// new JoystickButton(manipulatorController, PASS_TO_OUTTAKE_STICK)
// .onTrue(new PassToOuttake(intakeShooter));
// new JoystickButton(manipulatorController, PASS_TO_INTAKE_STICK)
// .onTrue(new PassToIntake(intakeShooter));
// new JoystickButton(manipulatorController, SPEAKER_POS)
// .onTrue(new ArmToPos(arm, SPEAKER_ANGLE_RAD));
// new POVButton(manipulatorController, UP_D_PAD)
// .onTrue(new ArmToPos(arm, CLIMB_POS));
// new POVButton(manipulatorController, DOWN_D_PAD).onTrue(new Climb(arm));
// new POVButton(manipulatorController, LEFT_D_PAD)
// .onTrue(new ArmToPos(arm, PODIUM_ANGLE_RAD));

}

Expand Down Expand Up @@ -329,80 +329,80 @@ private Trigger axisTrigger(GenericHID controller, Axis axis) {

private void registerAutoCommands() {
//// AUTO-USABLE COMMANDS
NamedCommands.registerCommand("Intake", new Intake(intakeShooter));
NamedCommands.registerCommand("Eject", new Eject(intakeShooter));
// NamedCommands.registerCommand("Intake", new Intake(intakeShooter));
// NamedCommands.registerCommand("Eject", new Eject(intakeShooter));

// NamedCommands.registerCommand("ArmToSpeaker", new MoveToPos(arm,
// Armc.SPEAKER_ANGLE_RAD, 0));
NamedCommands.registerCommand("ArmToAmp",
new ArmToPos(arm, Armc.AMP_ANGLE_RAD));
NamedCommands.registerCommand("ArmToSubwoofer",
new ArmToPos(arm, Armc.SUBWOOFER_ANGLE_RAD));
NamedCommands.registerCommand("ArmToPodium",
new ArmToPos(arm, Armc.PODIUM_ANGLE_RAD));
NamedCommands.registerCommand("ArmToGround",
new ArmToPos(arm, GROUND_INTAKE_POS));
// NamedCommands.registerCommand("ArmToAmp",
// new ArmToPos(arm, Armc.AMP_ANGLE_RAD));
// NamedCommands.registerCommand("ArmToSubwoofer",
// new ArmToPos(arm, Armc.SUBWOOFER_ANGLE_RAD));
// NamedCommands.registerCommand("ArmToPodium",
// new ArmToPos(arm, Armc.PODIUM_ANGLE_RAD));
// NamedCommands.registerCommand("ArmToGround",
// new ArmToPos(arm, GROUND_INTAKE_POS));

NamedCommands.registerCommand("RampRPMAuton",
new RampRPMAuton(intakeShooter));
// NamedCommands.registerCommand("RampRPMAuton",
// new RampRPMAuton(intakeShooter));

NamedCommands.registerCommand("SwitchRPMShoot",
new Outtake(intakeShooter, arm));
// NamedCommands.registerCommand("SwitchRPMShoot",
// new Outtake(intakeShooter, arm));

// NamedCommands.registerCommand("AutonRuinerShoot", new
// AutonRuinerShoot(intakeShooter));
// NamedCommands.registerCommand("IntakeAutonRuiner", new
// IntakeAutonRuiner(intakeShooter));

NamedCommands.registerCommand("AutonRuinerShootAndIntake",
new AutonRuinerShootAndIntake(intakeShooter, arm));

NamedCommands.registerCommand("PassToOuttake",
new PassToOuttake(intakeShooter));
NamedCommands.registerCommand("AimArmSpeakerMT2",
new AimArmSpeaker(arm, limelight));
NamedCommands.registerCommand("AlignToAprilTagMegaTag2",
new AlignToApriltag(drivetrain, limelight, 0.0));
NamedCommands.registerCommand("Shoot", new SequentialCommandGroup(
new ParallelDeadlineGroup(
new WaitCommand(3.0),
new SequentialCommandGroup(
// TODO: Use Align To Drivetrain
// new AlignDrivetrain(drivetrain),
new ParallelCommandGroup(
new AlignToApriltag(drivetrain, limelight, 0.0),
new AimArmSpeaker(arm, limelight),
new RampRPMAuton(intakeShooter)),
new PassToOuttake(intakeShooter),
new ArmToPos(arm, GROUND_INTAKE_POS)))));
NamedCommands.registerCommand("ShootSubwoofer",
new SequentialCommandGroup(new ParallelCommandGroup(
new ArmToPos(arm,
Armc.SUBWOOFER_ANGLE_RAD),
new RampRPMAuton(intakeShooter)),
new PassToOuttake(intakeShooter),
new ArmToPos(arm, GROUND_INTAKE_POS)));
NamedCommands.registerCommand("Limelight Intake CCW",
new ParallelCommandGroup(new Intake(intakeShooter),
new AutoMATICALLYGetNote(drivetrain, limelight,
intakeShooter, 1)));
NamedCommands.registerCommand("Limelight Intake CW",
new ParallelCommandGroup(new Intake(intakeShooter),
new AutoMATICALLYGetNote(drivetrain, limelight,
intakeShooter, -1)));

NamedCommands.registerCommand("Limelight Intake Straight",
new ParallelCommandGroup(new Intake(intakeShooter),
new AutoMATICALLYGetNote(drivetrain, limelight,
intakeShooter, 0)));

NamedCommands.registerCommand("StopIntake",
new InstantCommand(intakeShooter::stopIntake));
NamedCommands.registerCommand("StopOutake",
new InstantCommand(intakeShooter::stopOuttake));
NamedCommands.registerCommand("StopBoth",
new ParallelCommandGroup(new InstantCommand(intakeShooter::stopIntake),
new InstantCommand(intakeShooter::stopOuttake)));
// NamedCommands.registerCommand("AutonRuinerShootAndIntake",
// new AutonRuinerShootAndIntake(intakeShooter, arm));

// NamedCommands.registerCommand("PassToOuttake",
// new PassToOuttake(intakeShooter));
// NamedCommands.registerCommand("AimArmSpeakerMT2",
// new AimArmSpeaker(arm, limelight));
// NamedCommands.registerCommand("AlignToAprilTagMegaTag2",
// new AlignToApriltag(drivetrain, limelight, 0.0));
// NamedCommands.registerCommand("Shoot", new SequentialCommandGroup(
// new ParallelDeadlineGroup(
// new WaitCommand(3.0),
// new SequentialCommandGroup(
// // TODO: Use Align To Drivetrain
// // new AlignDrivetrain(drivetrain),
// new ParallelCommandGroup(
// new AlignToApriltag(drivetrain, limelight, 0.0),
// new AimArmSpeaker(arm, limelight),
// new RampRPMAuton(intakeShooter)),
// new PassToOuttake(intakeShooter),
// new ArmToPos(arm, GROUND_INTAKE_POS)))));
// NamedCommands.registerCommand("ShootSubwoofer",
// new SequentialCommandGroup(new ParallelCommandGroup(
// new ArmToPos(arm,
// Armc.SUBWOOFER_ANGLE_RAD),
// new RampRPMAuton(intakeShooter)),
// new PassToOuttake(intakeShooter),
// new ArmToPos(arm, GROUND_INTAKE_POS)));
// NamedCommands.registerCommand("Limelight Intake CCW",
// new ParallelCommandGroup(new Intake(intakeShooter),
// new AutoMATICALLYGetNote(drivetrain, limelight,
// intakeShooter, 1)));
// NamedCommands.registerCommand("Limelight Intake CW",
// new ParallelCommandGroup(new Intake(intakeShooter),
// new AutoMATICALLYGetNote(drivetrain, limelight,
// intakeShooter, -1)));

// NamedCommands.registerCommand("Limelight Intake Straight",
// new ParallelCommandGroup(new Intake(intakeShooter),
// new AutoMATICALLYGetNote(drivetrain, limelight,
// intakeShooter, 0)));

// NamedCommands.registerCommand("StopIntake",
// new InstantCommand(intakeShooter::stopIntake));
// NamedCommands.registerCommand("StopOutake",
// new InstantCommand(intakeShooter::stopOuttake));
// NamedCommands.registerCommand("StopBoth",
// new ParallelCommandGroup(new InstantCommand(intakeShooter::stopIntake),
// new InstantCommand(intakeShooter::stopOuttake)));
}

private void setupAutos() {
Expand Down
Loading

0 comments on commit 6fbf25f

Please sign in to comment.