Skip to content
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

Rewrite aimerio #40

Merged
merged 7 commits into from
Oct 9, 2024
Merged

Rewrite aimerio #40

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
172ba01
WIP - (Untested)Rewrite aimerIO to fit high-key robot
aidnem Oct 8, 2024
9f74cde
WIP - (Untested) Update aimer MotionMagic profile to be super slow fo…
aidnem Oct 8, 2024
c996596
WIP - Format
aidnem Oct 8, 2024
e12d5ea
Start implementing tuning-aimer test mode
aidnem Oct 9, 2024
2c9f294
Add TuneG and TuneS to aimer tuning mode. NEED TO UPDATE TO USE COPPE…
aidnem Oct 9, 2024
a076d7c
Disable shooter
charlesmackenzie Oct 9, 2024
32359e9
Basic aimer tuning
charlesmackenzie Oct 9, 2024
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
136 changes: 136 additions & 0 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
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.InstantCommand;
Expand All @@ -14,6 +16,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 @@ -34,6 +37,8 @@
import frc.robot.subsystems.scoring.ShooterIO;
import frc.robot.subsystems.scoring.ShooterIOSim;
import frc.robot.subsystems.scoring.ShooterIOTalonFX;
import frc.robot.utils.feedforward.TuneG;
import frc.robot.utils.feedforward.TuneS;

public class RobotContainer {
PhoenixDrive drive;
Expand All @@ -49,8 +54,11 @@ public class RobotContainer {

VisionLocalizer tagVision;

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

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

Expand Down Expand Up @@ -163,6 +171,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 @@ -315,6 +331,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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading