Skip to content

Commit

Permalink
Improved controls (#38)
Browse files Browse the repository at this point in the history
* rumble subsystem + redside supplier

* moved all arm commands to ArmToAngle factories

* deleted unused files that caused chaos and confusion

* more deletion for those who cause chaos

* return if interrupted command

* ratchette disengage

* ritvik ratchette (RR)

* bundt shot (rsetty + alu)

* Bundt Shot working (yes)

* intake returns back up

* auton start pos and current limits

* intermap change + removing unnecessary comments

* remove rumble

* kalman tweak (trust vision more)

* changed from xboxcontroller to our own implementation

* reduces cpu usage

* updated defualt arm angle to 56.12 deg

* updated rachette and added rumble back

* updated intake current lim (as per Jeff's request)

* got ratchette working and added timeout to intake for auton

* changed intermap and introduced logging for arm angle

* updated arm constants to reflect updated weight

* added constant to improve arm aiming

* updated arm angle and logging

* commit for the intermap +

* intermap changes (working from far)

* fixed arm alignment shooting issue. WE ARE CONSISTIENT (except when odo is off)

* added commented constant voltage control for preshooter mtoro

* added square drive and changed preshooter motor to constant voltage for shoot

* updated preshooter wheel voltage

* added reset arm position and fixed square input

* added SignalLogger Logs

* changed signallogger

* changed mihir controls to change arm angles to pov buttons

* spotless and removed fireauton error

* simplified intermap code

* added comment to shooting speed

* added LED code

---------

Co-authored-by: pastamaple73 <[email protected]>
Co-authored-by: Rawpower9 <[email protected]>
Co-authored-by: chivesonions <[email protected]>
Co-authored-by: ritvik <[email protected]>
  • Loading branch information
5 people authored Mar 9, 2024
1 parent e003c6b commit 8bba71a
Show file tree
Hide file tree
Showing 16 changed files with 339 additions and 107 deletions.
2 changes: 1 addition & 1 deletion .vscode/settings.json
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,6 @@
"spotlessGradle.diagnostics.enable": true,
"spotlessGradle.format.enable": true,
"editor.codeActionsOnSave": {
"source.fixAll.spotlessGradle": "never"
"source.fixAll.spotlessGradle": "explicit"
},
}
53 changes: 38 additions & 15 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,14 @@
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static class LED {
public static final int LED_STRIP_LENGTH = 34;
public static final int LED_STRIP_PORT = 7;
public static final int[] PURE_RED = {0, 100, 100};
public static final int[] PURE_BLUE = {201, 100, 100};
public static final int[] PURE_YELLOW = {61, 100, 100};
}

public static class OperatorConstants {
public static final int kDriverControllerPort = 0;
public static final int PS4_CONTROLLER_PORT_1 = 3;
Expand Down Expand Up @@ -51,8 +59,9 @@ public static final class Peter {

public static final double INTAKE_WHEEL_SPEED_RPS = 200; // Intake gear ratio: 2:1
public static final double ROTATIONS_TO_SHOOTER = 300; // Preshooter gear ratio: 4:1
public static final double SHOOT_WHEEL_SPEED_RPS = 4500.0 / 60.0;

public static final double SHOOT_WHEEL_SPEED_RPS =
3500.0 / 60.0; // TODO: Add right and left motor RPS, and change it to not max
public static final double PRESHOOTER_WHEEL_VOLTAGE = 9;
public static final String CANBUS_NAME = "rio";

public static final double INTAKE_GEAR_RATIO = 2;
Expand Down Expand Up @@ -96,24 +105,38 @@ public static final class Arm {
* INTEGRATED_ABSOLUTE_CONVERSION_FACTOR; // 130.63563333333335;
public static final double ABSOLUTE_ENCODER_HORIZONTAL = 0.6547;
public static final double ABSOLUTE_HORIZONTAL_OFFSET = 0.05;

public static final InterpolatingDoubleTreeMap INTERMAP1 = new InterpolatingDoubleTreeMap();
public static double ARM_INTERMAP_OFFSET = 4;
public static final InterpolatingDoubleTreeMap INTERMAP = new InterpolatingDoubleTreeMap();

static {
INTERMAP1.put(
1.34, 6d + 4); // measurements of distance are from front of robot bumper to wall
INTERMAP1.put(2.1, 17d + 4);
INTERMAP1.put(Units.feetToMeters(9) + Units.inchesToMeters(17), 23.5d + 4.5);
UPDATE_INTERMAP();
// INTERMAP.put(
// 1.34,
// 6d + ARM_INTERMAP_OFFSET); // measurements of distance are from front of robot bumper
// to
// // wall
// INTERMAP.put(2.1, 17d + ARM_INTERMAP_OFFSET);
// INTERMAP.put(Units.feetToMeters(9) + Units.inchesToMeters(17), 23.5d +
// ARM_INTERMAP_OFFSET);
}

public static final InterpolatingDoubleTreeMap INTERMAP2 = new InterpolatingDoubleTreeMap();

static {
INTERMAP2.put(
1.34, 6d + 5); // measurements of distance are from front of robot bumper to wall
INTERMAP2.put(2.1, 17d + 5);
INTERMAP2.put(Units.feetToMeters(9) + Units.inchesToMeters(17), 23.5d + 5);
public static void UPDATE_INTERMAP() {
INTERMAP.clear();
INTERMAP.put(
1.34,
6d + ARM_INTERMAP_OFFSET); // measurements of distance are from front of robot bumper to
// wall
INTERMAP.put(2.1, 17d + ARM_INTERMAP_OFFSET);
INTERMAP.put(Units.feetToMeters(9) + Units.inchesToMeters(17), 23.5d + ARM_INTERMAP_OFFSET);
}

// public static final InterpolatingDoubleTreeMap INTERMAP2 = new InterpolatingDoubleTreeMap();
// static {
// INTERMAP2.put(1.34, 6d + 5); // measurements of distance are from front of robot bumper to
// wall
// INTERMAP2.put(2.1, 17d + 5);
// INTERMAP2.put(Units.feetToMeters(9) + Units.inchesToMeters(17), 23.5d + 5);
// }
}

public static class OI {
Expand Down
7 changes: 5 additions & 2 deletions src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.subsystems.ArmSubsystem;
import frc.robot.subsystems.LightsSubsystem;
import frc.robot.subsystems.PeterSubsystem;
import frc.robot.subsystems.PhotonVision;
import frc.robot.subsystems.SwerveSubsystem;
Expand All @@ -35,6 +36,8 @@ public class Robot extends TimedRobot {
private final SwerveSubsystem driveTrain = SwerveSubsystem.getInstance();
private final ArmSubsystem armSubsystem = ArmSubsystem.getInstance();
private final PeterSubsystem peterSubsystem = PeterSubsystem.getInstance();
private LightsSubsystem lightsSubsystem = LightsSubsystem.getInstance();

private RobotContainer m_robotContainer;
private static Matrix<N3, N1> visionMatrix = new Matrix<N3, N1>(Nat.N3(), Nat.N1());

Expand All @@ -44,8 +47,6 @@ public class Robot extends TimedRobot {
*/
@Override
public void robotInit() {
SignalLogger.setPath("/home/lvuser/logs/");
SignalLogger.start();
CameraServer.startAutomaticCapture(0);
visionMatrix.set(0, 0, 0);
visionMatrix.set(1, 0, 0.2d);
Expand Down Expand Up @@ -182,5 +183,7 @@ public void simulationPeriodic() {}

private void absoluteInit() {
RobotContainer.setAlliance();
SignalLogger.setPath("/home/lvuser/logs/");
SignalLogger.start();
}
}
10 changes: 6 additions & 4 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
import frc.robot.commands.ArmCommands.ArmToAngleCmd;
import frc.robot.commands.Auton.MoveToTarget;
import frc.robot.commands.Auton.RatchetteDisengage;
import frc.robot.commands.DebugCommands.AlterArmValues;
import frc.robot.commands.PeterCommands.ShootNoWarmup;
import frc.robot.commands.PeterCommands.SpinUpShooter;
import frc.robot.commands.SwerveCommands.SwerveJoystickCommand;
Expand Down Expand Up @@ -121,8 +122,7 @@ private void configureBindings() {
frontBackFunction,
leftRightFunction,
speedFunction,
redside,
() -> joystickB.getLeftX() >= 0.05));
redside));

joystickA
.a()
Expand All @@ -136,8 +136,7 @@ private void configureBindings() {
frontBackFunction,
leftRightFunction,
speedFunction,
redside,
() -> joystickB.getLeftX() >= 0.05));
redside));

joystickA
.a()
Expand Down Expand Up @@ -240,6 +239,9 @@ private void configureBindings() {
5.5),
Rotation2d.fromDegrees(!redAlliance ? 0 : 180))))
.andThen(new PrintCommand("pov worked")));

joystickB.povLeft().onTrue(new AlterArmValues(-0.25));
joystickB.povRight().onTrue(new AlterArmValues(0.25));
}

// Constructs a Pose2d array of the note locations by a specific indexing so they can be accessed
Expand Down
21 changes: 4 additions & 17 deletions src/main/java/frc/robot/commandGroups/AimAtSpeaker.java
Original file line number Diff line number Diff line change
Expand Up @@ -20,12 +20,10 @@ public AimAtSpeaker(
Supplier<Double> speedFunction,
double headingToleranceDegrees,
double armToleranceDegrees,
Supplier<Boolean> redSide,
Supplier<Boolean> increaseAngle) {
Supplier<Boolean> redSide) {
addCommands(
new SpinUpShooter(peter),
ArmToAngleCmd.aimAtSpeaker(arm, swerve, redSide, increaseAngle)
.withTolerance(armToleranceDegrees),
ArmToAngleCmd.aimAtSpeaker(arm, swerve, redSide).withTolerance(armToleranceDegrees),
SwerveLockedAngleCmd.fromPoseMirrored(
frontBackFunction,
leftRightFunction,
Expand All @@ -44,18 +42,7 @@ public AimAtSpeaker(
Supplier<Double> frontBackFunction,
Supplier<Double> leftRightFunction,
Supplier<Double> speedFunction,
Supplier<Boolean> redSide,
Supplier<Boolean> increaseAngle) {
this(
peter,
arm,
swerve,
frontBackFunction,
leftRightFunction,
speedFunction,
-1,
-1,
redSide,
increaseAngle);
Supplier<Boolean> redSide) {
this(peter, arm, swerve, frontBackFunction, leftRightFunction, speedFunction, -1, -1, redSide);
}
}
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/commandGroups/BundtShot.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.commands.ArmCommands.ArmToAngleCmd;
import frc.robot.commands.DebugCommands.ResetArm;
import frc.robot.commands.DebugCommands.Rumble;
import frc.robot.commands.PeterCommands.ShootNoWarmup;
import frc.robot.commands.PeterCommands.SpinUpShooter;
Expand All @@ -16,6 +17,7 @@ public BundtShot(
ArmSubsystem armSubsystem,
JoystickSubsystem joystickSubsystem) {
addCommands(
new ResetArm(armSubsystem),
new ParallelCommandGroup(
new SpinUpShooter(peterSubsystem),
ArmToAngleCmd.toBundt(armSubsystem).withTolerance(1)),
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/commandGroups/FireAuton.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.Constants;
import frc.robot.commands.DebugCommands.ResetArm;
import frc.robot.commands.PeterCommands.ShootNoWarmup;
import frc.robot.commands.SwerveCommands.SwerveLockedAngleCmd;
import frc.robot.subsystems.ArmSubsystem;
Expand All @@ -18,6 +19,7 @@ public FireAuton(
double tolerance,
Supplier<Boolean> redside) {
addCommands(
new ResetArm(armSubsystem),
new AimAtSpeaker(
peterSubsystem,
armSubsystem,
Expand All @@ -27,8 +29,7 @@ public FireAuton(
() -> 0.0,
tolerance,
2,
redside,
() -> false)
redside)
.withTimeout(1.0),
new ParallelCommandGroup(
new ShootNoWarmup(peterSubsystem, true).withTimeout(0.5),
Expand Down
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/commandGroups/FireTeleop.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.Constants;
import frc.robot.commands.ArmCommands.ArmToAngleCmd;
import frc.robot.commands.DebugCommands.ResetArm;
import frc.robot.commands.DebugCommands.Rumble;
import frc.robot.commands.PeterCommands.ShootNoWarmup;
import frc.robot.commands.SwerveCommands.SwerveLockedAngleCmd;
Expand All @@ -23,9 +24,9 @@ public FireTeleop(
Supplier<Double> frontBackFunction,
Supplier<Double> leftRightFunction,
Supplier<Double> speedFunction,
Supplier<Boolean> redside,
Supplier<Boolean> increaseAngle) {
Supplier<Boolean> redside) {
addCommands(
new ResetArm(armSubsystem),
new AimAtSpeaker(
peterSubsystem,
armSubsystem,
Expand All @@ -35,12 +36,11 @@ public FireTeleop(
speedFunction,
5,
1,
redside,
increaseAngle),
redside),
new ParallelCommandGroup(
new ShootNoWarmup(peterSubsystem, false).withTimeout(1),
Rumble.withNoBlock(joystickSubsystem, 1, 1, 0.25),
ArmToAngleCmd.aimAtSpeaker(armSubsystem, driveTrain, redside, increaseAngle),
ArmToAngleCmd.aimAtSpeaker(armSubsystem, driveTrain, redside),
SwerveLockedAngleCmd.fromPoseMirrored(
frontBackFunction,
leftRightFunction,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -67,16 +67,11 @@ public static ArmToAngleCmd toAmp(ArmSubsystem arm) {
}

public static ArmToAngleCmd aimAtSpeaker(
ArmSubsystem arm,
SwerveSubsystem swerveSubsystem,
Supplier<Boolean> redside,
Supplier<Boolean> increaseAngle) {
ArmSubsystem arm, SwerveSubsystem swerveSubsystem, Supplier<Boolean> redside) {
return new ArmToAngleCmd(
() ->
ArmSubsystem.calculateAngleToSpeaker(
swerveSubsystem.getState().Pose.getTranslation(),
redside.get(),
increaseAngle.get()),
swerveSubsystem.getState().Pose.getTranslation(), redside.get()),
arm);
}

Expand Down
33 changes: 33 additions & 0 deletions src/main/java/frc/robot/commands/DebugCommands/AlterArmValues.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
package frc.robot.commands.DebugCommands;

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants;

public class AlterArmValues extends Command {
private double increaseBy;

public AlterArmValues(double increaseBy) {
this.increaseBy = increaseBy;
}

// Called when the command is initially scheduled.
@Override
public void initialize() {
Constants.Arm.ARM_INTERMAP_OFFSET += increaseBy;
Constants.Arm.UPDATE_INTERMAP();
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}

// Returns true when the command should end.
@Override
public boolean isFinished() {
return true;
}
}
32 changes: 32 additions & 0 deletions src/main/java/frc/robot/commands/DebugCommands/ResetArm.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
package frc.robot.commands.DebugCommands;

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.ArmSubsystem;

public class ResetArm extends Command {
private ArmSubsystem armSubsystem;

public ResetArm(ArmSubsystem armSubsystem) {
this.armSubsystem = armSubsystem;
}

// Called when the command is initially scheduled.
@Override
public void initialize() {
armSubsystem.resetPosition();
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}

// Returns true when the command should end.
@Override
public boolean isFinished() {
return true;
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,10 @@ public void execute() {
ySpeed /= length;
}

// Apply Square (will be [0,1] since `speed` is [0,1])
xSpeed = xSpeed * xSpeed * Math.signum(xSpeed);
ySpeed = ySpeed * ySpeed * Math.signum(ySpeed);

// 3. Apply deadband
xSpeed = Math.abs(xSpeed) > Constants.OI.LEFT_JOYSTICK_DEADBAND ? xSpeed : 0.0;
ySpeed = Math.abs(ySpeed) > Constants.OI.LEFT_JOYSTICK_DEADBAND ? ySpeed : 0.0;
Expand Down
Loading

0 comments on commit 8bba71a

Please sign in to comment.