Skip to content

Commit

Permalink
shooter amp and elevator clamps
Browse files Browse the repository at this point in the history
  • Loading branch information
@Nins97
Nins97 committed Feb 23, 2024
1 parent a65e361 commit 9fda108
Show file tree
Hide file tree
Showing 7 changed files with 51 additions and 51 deletions.
16 changes: 9 additions & 7 deletions src/main/java/frc/robot/BuildConstants.java
View file
Original file line number Diff line number Diff line change
@@ -1,17 +1,19 @@
package frc.robot;

/** Automatically generated file containing build version information. */
/**
* Automatically generated file containing build version information.
*/
public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "Competition2024";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 119;
public static final String GIT_SHA = "e671beccb907bb2363fb24966580cde710df9cc4";
public static final String GIT_DATE = "2024-02-21 19:06:30 MST";
public static final int GIT_REVISION = 121;
public static final String GIT_SHA = "a65e3613f3d7f09347f4d15f17ca5ec3295fc206";
public static final String GIT_DATE = "2024-02-22 13:34:48 MST";
public static final String GIT_BRANCH = "workingongettingready";
public static final String BUILD_DATE = "2024-02-22 08:42:25 MST";
public static final long BUILD_UNIX_TIME = 1708616545688L;
public static final String BUILD_DATE = "2024-02-22 18:30:45 MST";
public static final long BUILD_UNIX_TIME = 1708651845565L;
public static final int DIRTY = 1;

private BuildConstants() {}
private BuildConstants(){}
}
12 changes: 7 additions & 5 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ public static class SlapdownConstants {

public static final int ROTATION_LIMIT_SWITCH_ID = 1;

public static final double FEEDER_VOLTAGE = 6.0;
public static final double FEEDER_VOLTAGE = 9.0;

public static final TunablePIDGains ROTATION_GAINS =
new TunablePIDGains("/slapdown/rotation/gains", 4.0, 0, 0, MiscConstants.TUNING_MODE);
Expand Down Expand Up @@ -240,8 +240,8 @@ public static class WristConstants {
public static final TunableTrapezoidalProfileGains TRAPEZOIDAL_PROFILE_GAINS =
new TunableTrapezoidalProfileGains(
"/wrist/trapGains",
Units.rotationsToRadians(0.5),
Units.rotationsToRadians(0.5),
Units.rotationsToRadians(1.5),
Units.rotationsToRadians(1.5),
MiscConstants.TUNING_MODE);
}

Expand All @@ -251,6 +251,8 @@ public static class TransportConstants {
public static final int STALL_MOTOR_CURRENT = 45;
public static final int FREE_MOTOR_CURRENT = 25;
public static final double TRANSPORT_VOLTAGE = 6.0;

public static final double TRANSPORT_SHOOTER_VOLTAVE = 12;
public static final int SHOOTER_SENSOR_ID = 8;
}

Expand Down Expand Up @@ -287,8 +289,8 @@ public static class TeleopConstants {
private TeleopConstants() {}

public static final boolean OPEN_LOOP_DRIVETRAIN = true;
public static final double TRANSLATION_RATE_LIMIT_METERS_SECOND_SQUARED = 10.0;
public static final double ANGULAR_RATE_LIMIT_RADIANS_SECOND_SQUARED = 5.0 * Math.PI;
public static final double TRANSLATION_RATE_LIMIT_METERS_SECOND_SQUARED = 15.0;
public static final double ANGULAR_RATE_LIMIT_RADIANS_SECOND_SQUARED = 8.0 * Math.PI;
public static final double MINIMUM_VELOCITY_METERS_SECOND = 0.10;
public static final double MINIMUM_ANGULAR_VELOCITY_RADIANS_SECOND = 0.10;
}
Expand Down
55 changes: 23 additions & 32 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
Expand Down Expand Up @@ -144,6 +145,7 @@ private void configureAutos() {
Commands.parallel(
wristSubsystem.setPositonCommand(Rotation2d.fromDegrees(40.0)),
elevatorSubsystem.setElevatorPositionCommand(Units.inchesToMeters(8.0))));
autoCommand.addOption("run feeder", slapdownSuperstructure.getSlapdownFeederSubsystem().setVoltageCommand(6));

autoCommand.addOption(
"Shooter Quastatic Backward Command",
Expand All @@ -168,32 +170,32 @@ private void configureAutos() {

private void configureDriverBindings() {
configureDriving();
driverController.y().onTrue(Commands.parallel(elevatorSubsystem.setElevatorPositionCommand(Constants.ScoringConstants.AMP_ELEVATOR_HEIGHT), wristSubsystem.setPositonCommand(new Rotation2d(Constants.ScoringConstants.AMP_WRIST_ANGLE))));
driverController
.home()
.onTrue(
RaiderCommands.runOnceAllowDisable(driveSubsystem::zeroHeading)
.withName("ZeroHeading"));
driverController
.leftTrigger()
.whileTrue(
transportSubsystem.setVoltageCommand(Constants.TransportConstants.TRANSPORT_VOLTAGE));
.whileTrue(Commands.sequence(elevatorSubsystem.setElevatorPositionCommand(Units.inchesToMeters(3)),
transportSubsystem.setVoltageCommand(Constants.TransportConstants.TRANSPORT_SHOOTER_VOLTAVE)));
driverController.minus().whileTrue(new LockModulesCommand(driveSubsystem).repeatedly());
Command intakeAndFeedUntilDone =
Commands.parallel(
intakeSubsystem.setIntakeVoltageCommand(Constants.IntakeConstants.INTAKE_VOLTAGE),
transportSubsystem.setVoltageCommand(
Constants.TransportConstants.TRANSPORT_VOLTAGE))
.until(transportSubsystem::atSensor)
.unless(transportSubsystem::atSensor);
Constants.TransportConstants.TRANSPORT_VOLTAGE)).until(transportSubsystem::atSensor)
.unless(transportSubsystem::atSensor);
driverController
.leftBumper()
.rightStick()
.whileTrue(
Commands.parallel(
slapdownSuperstructure.setDownAndRunCommand(),
intakeAndFeedUntilDone.asProxy(),
elevatorSubsystem.setElevatorPositionCommand(0),
elevatorSubsystem.setElevatorPositionCommand(Units.inchesToMeters(1)),
wristSubsystem.setPositonCommand(new Rotation2d(0))));
driverController.leftBumper().onFalse(slapdownSuperstructure.setUpCommand());
driverController.rightStick().onFalse(slapdownSuperstructure.setUpCommand());
// TODO: Speaker centric
driverController.rightTrigger().whileTrue(Commands.none());
// TODO: Amp auto align
Expand All @@ -206,40 +208,29 @@ private void configureDriverBindings() {
private void configureOperatorBindings() {
operatorController
.triangle()
.onTrue(Commands.sequence(shooterSubsystem.runVelocityCommand(10000.0 / 60)));
.onTrue(Commands.parallel(shooterSubsystem.runVelocityCommand((10000.0 / 60)), Commands.waitUntil(shooterSubsystem::inTolerance).andThen(Commands.runEnd(() ->
operatorController.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0),
() -> operatorController.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0)
))));

operatorController.x().onTrue(shooterSubsystem.setVoltageCommand(0));
operatorController
.leftTrigger()
.onTrue(
new IntakeToShooterCommand(
elevatorSubsystem,
intakeSubsystem,
wristSubsystem,
transportSubsystem,
shooterSubsystem));


// operatorController
// .rightStick()
// .whileTrue(elevatorSubsystem.runElevatorCommand(operatorController.getRightY()));

operatorController
.rightTrigger()
.onTrue(
new ShootAtAngleCommand(
shooterSubsystem, transportSubsystem, wristSubsystem, SHOOTING_ANGLE));
operatorController
.leftTrigger()
.onTrue(
new IntakeToShooterCommand(
elevatorSubsystem,
intakeSubsystem,
wristSubsystem,
transportSubsystem,
shooterSubsystem));
operatorController
.triangle()
.onTrue(
new AmpPlaceCommand(
elevatorSubsystem, wristSubsystem, shooterSubsystem, transportSubsystem));
operatorController.leftTrigger().onTrue(Commands.parallel(elevatorSubsystem.setElevatorPositionCommand(Constants.ElevatorConstants.ELEVATOR_MIN_HEIGHT), wristSubsystem.setPositonCommand(Constants.WristConstants.WRIST_MIN)));
// operatorController
// .triangle()
// .onTrue(
// new AmpPlaceCommand(
// elevatorSubsystem, wristSubsystem, shooterSubsystem, transportSubsystem));
operatorController
.share()
.whileTrue(
Expand Down
9 changes: 4 additions & 5 deletions src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.RelativeEncoder;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.DigitalInput;
Expand All @@ -19,10 +20,8 @@
import frc.robot.telemetry.types.DoubleTelemetryEntry;
import frc.robot.telemetry.types.EventTelemetryEntry;
import frc.robot.telemetry.wrappers.TelemetryCANSparkMax;
import frc.robot.utils.Alert;
import frc.robot.utils.ConfigurationUtils;
import frc.robot.utils.*;
import frc.robot.utils.ConfigurationUtils.StringFaultRecorder;
import frc.robot.utils.RaiderCommands;

public class ElevatorSubsystem extends SubsystemBase {

Expand Down Expand Up @@ -151,7 +150,7 @@ public boolean isHomed() {
}

public Command setElevatorPositionCommand(double position) {
// If we are not homed, then do not try to run closed loop. that will end poorly
double positionClamped = MathUtil.clamp(position, ELEVATOR_MAX_HEIGHT, ELEVATOR_MIN_HEIGHT);
return RaiderCommands.ifCondition(this::isHomed)
.then(
this.run(
Expand All @@ -164,7 +163,7 @@ public Command setElevatorPositionCommand(double position) {
.beforeStarting(
() -> {
controller.reset(getPosition(), elevatorEncoder.getVelocity());
controller.setGoal(position);
controller.setGoal(positionClamped);
}))
.otherwise(Commands.none());
}
Expand Down
4 changes: 3 additions & 1 deletion src/main/java/frc/robot/subsystems/slapdown/SlapdownFeederSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.telemetry.types.DoubleTelemetryEntry;
import frc.robot.telemetry.types.EventTelemetryEntry;
import frc.robot.telemetry.wrappers.TelemetryCANSparkMax;
import frc.robot.utils.Alert;
Expand All @@ -16,7 +17,7 @@
public class SlapdownFeederSubsystem extends SubsystemBase {
private static final Alert feederMotorAlert =
new Alert("Slapdown feeder motor had a fault initializing", Alert.AlertType.ERROR);

DoubleTelemetryEntry voltageReq = new DoubleTelemetryEntry("/slapdown/feeder/voltageReq", true);
private final TelemetryCANSparkMax feederMotor =
new TelemetryCANSparkMax(
FEEDER_MOTOR_ID,
Expand Down Expand Up @@ -89,6 +90,7 @@ private void configMotors() {
}

public void setFeederVoltage(double voltage) {
voltageReq.append(voltage);
feederMotor.setVoltage(voltage);
}

Expand Down
3 changes: 3 additions & 0 deletions src/main/java/frc/robot/subsystems/slapdown/SlapdownSuperstructure.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,9 @@ public Command setDownAndRunCommand() {
public SlapdownRotationSubsystem getSlapdownRotationSubsystem() {
return rotationSubsystem;
}
public SlapdownFeederSubsystem getSlapdownFeederSubsystem(){
return feederSubsystem;
}

public Command setUpCommand() {
return Commands.parallel(
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
import frc.robot.Constants;
import frc.robot.Constants.MiscConstants;
import frc.robot.FieldConstants;
import frc.robot.Robot;
import frc.robot.telemetry.types.*;
import frc.robot.telemetry.wrappers.TelemetryPigeon2;
import frc.robot.utils.Alert;
Expand Down Expand Up @@ -165,7 +166,7 @@ public SwerveDriveSubsystem(Function<Pose2d, List<EstimatedRobotPose>> cameraPos
kinematics, getGyroRotation(), getModulePositions(), new Pose2d());

// Start odometry thread
// Robot.getInstance().addPeriodic(this::updateOdometry, 1.0 / ODOMETRY_FREQUENCY);
Robot.getInstance().addPeriodic(this::updateOdometry, 1.0 / ODOMETRY_FREQUENCY);

stopMovement();
}
Expand Down

0 comments on commit 9fda108

Please sign in to comment.