Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin' into sensor-integration
Browse files Browse the repository at this point in the history
  • Loading branch information
@aidnem
aidnem committed Oct 9, 2024
2 parents 182a23a + 4ba686f commit 9b830ef
Show file tree
Hide file tree
Showing 13 changed files with 485 additions and 158 deletions.
136 changes: 136 additions & 0 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
package frc.robot;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
Expand All @@ -15,6 +17,7 @@
import frc.robot.constants.ModeConstants;
import frc.robot.constants.PhoenixDriveConstants;
import frc.robot.constants.PhoenixDriveConstants.AlignTarget;
import frc.robot.constants.ScoringConstants;
import frc.robot.constants.VisionConstants;
import frc.robot.subsystems.drive.PhoenixDrive;
import frc.robot.subsystems.drive.PhoenixDrive.SysIdRoutineType;
Expand All @@ -36,6 +39,8 @@
import frc.robot.subsystems.scoring.ShooterIOSim;
import frc.robot.subsystems.scoring.ShooterIOTalonFX;
import java.util.function.BooleanSupplier;
import frc.robot.utils.feedforward.TuneG;
import frc.robot.utils.feedforward.TuneS;

public class RobotContainer {
PhoenixDrive drive;
Expand All @@ -51,8 +56,11 @@ public class RobotContainer {

VisionLocalizer tagVision;

SendableChooser<String> testModeChooser = new SendableChooser<String>();

public RobotContainer() {
configureSubsystems();
configureModes();
configureBindings();
}

Expand Down Expand Up @@ -165,6 +173,14 @@ private void initScoring() {
}
}

private void configureModes() {
testModeChooser.setDefaultOption("Blank", "tuning");

testModeChooser.setDefaultOption("Aimer Tunig", "tuning-aimer");

SmartDashboard.putData("Test Mode Chooser", testModeChooser);
}

private void configureBindings() {
if (drive != null) {
drive.registerTelemetry(logger::telemeterize);
Expand Down Expand Up @@ -334,6 +350,126 @@ public void enabledInit() {
scoringSubsystem.setAction(ScoringAction.WAIT);
}

public void testInit() {
// Reset bindings
masher = new CommandXboxController(2);

switch (testModeChooser.getSelected()) {
case "tuning":
break;
case "tuning-aimer":
SmartDashboard.putNumber("Test-Mode/aimer/kP", ScoringConstants.aimerkP);
SmartDashboard.putNumber("Test-Mode/aimer/kI", ScoringConstants.aimerkI);
SmartDashboard.putNumber("Test-Mode/aimer/kD", ScoringConstants.aimerkD);

SmartDashboard.putNumber(
"Test-Mode/aimer/profileMaxVelocity", ScoringConstants.aimerCruiseVelocity);
SmartDashboard.putNumber(
"Test-Mode/aimer/profileMaxAcceleration",
ScoringConstants.aimerAcceleration);

SmartDashboard.putNumber("Test-Mode/aimer/setpointPosition", 0.0);
SmartDashboard.putNumber("Test-Mode/aimer/volts", 2.0);

scoringSubsystem.setAction(ScoringAction.OVERRIDE);

// TODO: Add Tunables to coppercore!
masher.a().onTrue(new TuneS(scoringSubsystem, 0));

masher.b().onTrue(new TuneG(scoringSubsystem, 0));

masher.y()
.onTrue(
new InstantCommand(
() ->
scoringSubsystem.setPID(
SmartDashboard.getNumber(
"Test-Mode/aimer/kP",
ScoringConstants.aimerkP),
SmartDashboard.getNumber(
"Test-Mode/aimer/kI",
ScoringConstants.aimerkI),
SmartDashboard.getNumber(
"Test-Mode/aimer/kD",
ScoringConstants.aimerkD),
0)))
.onTrue(
new InstantCommand(
() ->
scoringSubsystem.setMaxProfileProperties(
SmartDashboard.getNumber(
"Test-Mode/aimer/profileMaxVelocity",
ScoringConstants
.aimerCruiseVelocity),
SmartDashboard.getNumber(
"Test-Mode/aimer/profileMaxAcceleration",
ScoringConstants.aimerAcceleration),
0)))
.onTrue(
new InstantCommand(
() ->
scoringSubsystem.runToPosition(
SmartDashboard.getNumber(
"Test-Mode/aimer/setpointPosition",
0.0),
0)))
.onTrue(
new InstantCommand(
() ->
scoringSubsystem.setAction(
ScoringAction.TEMPORARY_SETPOINT)))
.onFalse(
new InstantCommand(
() -> scoringSubsystem.setAction(ScoringAction.OVERRIDE)));

// TODO: Figure out which of these we need
// masher.povUp()
// .onTrue(new InstantCommand(() -> scoringSubsystem.runToPosition(1.1, 0)))
// .onTrue(
// new InstantCommand(
// () ->
// scoringSubsystem.setAction(
// ScoringAction.TEMPORARY_SETPOINT)))
// .onFalse(
// new InstantCommand(
// () ->
// scoringSubsystem.setAction(ScoringAction.OVERRIDE)));

// masher.povDown()
// .onTrue(new InstantCommand(() -> scoringSubsystem.runToPosition(0.0, 0)))
// .onTrue(
// new InstantCommand(
// () ->
// scoringSubsystem.setAction(
// ScoringAction.TEMPORARY_SETPOINT)))
// .onFalse(
// new InstantCommand(
// () ->
// scoringSubsystem.setAction(ScoringAction.OVERRIDE)));

// masher.leftBumper()
// .onTrue(
// new InstantCommand(
// () ->
// scoringSubsystem.setVolts(
// SmartDashboard.getNumber(
// "Test-Mode/aimer/volts", 2.0),
// 0)))
// .onFalse(new InstantCommand(() -> scoringSubsystem.setVolts(0, 0)));

// masher.rightBumper()
// .onTrue(
// new InstantCommand(
// () ->
// scoringSubsystem.setVolts(
// -SmartDashboard.getNumber(
// "Test-Mode/aimer/volts", 2.0),
// 0)))
// .onFalse(new InstantCommand(() -> scoringSubsystem.setVolts(0, 0)));
break;
}
}

public Command getAutonomousCommand() {
return Commands.print("No autonomous command configured");
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/DriveWithJoysticks.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ public class DriveWithJoysticks extends Command {

private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();

private Deadband joystickDeadband = new Deadband();
// private Deadband joystickDeadband = new Deadband();

public DriveWithJoysticks(
PhoenixDrive drivetrain, CommandJoystick leftJoystick, CommandJoystick rightJoystick) {
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/constants/FeatureFlags.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,6 @@ public final class FeatureFlags {
// NOTE: A featureflag "runLocalizer" was removed from here recently.
// TODO: Figure out if we need this and add it back if necessary.
public static final boolean runLocalizer = false;
public static final boolean runIntake = true;
public static final boolean runScoring = false;
public static final boolean runIntake = false;
public static final boolean runScoring = true;
}
43 changes: 25 additions & 18 deletions src/main/java/frc/robot/constants/ScoringConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,14 @@
import java.util.HashMap;

public class ScoringConstants {
public static final double aimerkP = 17.0;
public static final double aimerkI = 10.0; // 5.0
public static final double aimerkP = 23.0;
public static final double aimerkI = 4.0; // 5.0
public static final double aimerkD = 0.0;

public static final double aimerkS = 0.265;
public static final double aimerkG = 0.1;
public static final double aimerkV = 1.51;
public static final double aimerkA = 0.01;
public static final double aimerkS = 0.0;
public static final double aimerkG = 0.2;
public static final double aimerkV = 0.0;
public static final double aimerkA = 0.0;

public static final double shooterkP = 0.05;
public static final double shooterkI = 0.2;
Expand All @@ -21,27 +21,34 @@ public class ScoringConstants {
public static final double shooterkV = 0.0095;
public static final double shooterkA = 0.0;

public static final int aimLeftMotorId = 16;
public static final int aimRightMotorId = 15;

public static final int shooterLeftMotorId = 11;
public static final int shooterRightMotorId = 12;
public static final int aimerMotorId = 9;

public static final int kickerMotorId = 13;

// TODO: REPLACE THIS WHEN THE ACTUAL SHOOTER IO IS MERGED
public static final int shooterLeftMotorId = 10;
public static final int shooterRightMotorId = 11;

public static final double shooterCurrentLimit = 120;
public static final double kickerCurrentLimit = 120;
public static final double aimerCurrentLimit = 60;

public static final int aimEncoderPort = 0;
public static final double aimerEncoderOffset = 1.75 - 0.01; // 0.027
public static final int aimerEncoderId = 13;
public static final double aimerEncoderOffset = 0.184570;

public static final double aimerEncoderToMechanismRatio = 1.0;
public static final double aimerRotorToSensorRatio = 90.0;

public static final double kickerIntakeVolts = 2.0;

public static final double aimPositionTolerance = 0.017;

public static final double aimAcceleration = 4.5; // TODO: 15.0
public static final double aimCruiseVelocity = 7.0; // TODO: 15.0
// These values have been reduced for tuning because we can't set a voltage limit on the motors
// anymore
// public static final double aimerAcceleration = 4.5; // TODO: 15.0
// public static final double aimerCruiseVelocity = 7.0; // TODO: 15.0
public static final double aimerAcceleration = 0.7;
public static final double aimerCruiseVelocity = 0.4;

public static final double shooterLowerVelocityMarginRPM = 50;
public static final double shooterUpperVelocityMarginRPM = 150;
Expand All @@ -53,8 +60,8 @@ public class ScoringConstants {

public static final double shooterAmpVelocityRPM = 2000;

public static final double aimMaxAngleRadians = 2 * Math.PI; // Math.PI / 2
public static final double aimMinAngleRadians = Math.PI;
public static final double aimMaxAngleRadians = 0.361328 - 0.184570; // Math.PI / 2
public static final double aimMinAngleRadians = -0.037598 - 0.184570;
public static final double aimAngleTolerance = 0.015;

public static final double maxAimIntake = 0.0;
Expand Down Expand Up @@ -89,7 +96,7 @@ public static HashMap<Double, Double> getAimerMap() {
return map;
}

public static final double aimerStaticOffset = 0.01;
public static final double aimerStaticOffset = 0.0;

// NOTE - This should be monotonically increasing
// Key - Distance in meters
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/constants/SensorConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,5 +3,5 @@
public final class SensorConstants {
// TODO: Find real values
public static final int indexerSensorPort = 0;
public static final int uptakeSensorPort = 0;
public static final int uptakeSensorPort = 1;
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/scoring/AimerIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ public default void updateInputs(AimerInputs inputs) {}

public default void applyOutputs(AimerOutputs outputs) {}

public default void setAimAngleRad(double angle, boolean newProfile) {}
public default void setAimAngleRad(double angle) {}

public default void controlAimAngleRad() {}

Expand Down
Loading

0 comments on commit 9b830ef

Please sign in to comment.