Skip to content

Commit

Permalink
tested everything
Browse files Browse the repository at this point in the history
  • Loading branch information
@cherriae
cherriae committed Mar 6, 2024
1 parent e9dd238 commit c27cb10
Show file tree
Hide file tree
Showing 7 changed files with 49 additions and 43 deletions.
36 changes: 19 additions & 17 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,31 +22,31 @@ public final class Constants {
public static final Alliance SAFE_ALLIANCE = Alliance.Red;

public static class CAN {
public static final int DRIVE_FRONT_LEFT = 5;
public static final int ROT_FRONT_LEFT = 6;
public static final int DRIVE_FRONT_LEFT = 1;
public static final int ROT_FRONT_LEFT = 2;

public static final int DRIVE_FRONT_RIGHT = 7;
public static final int ROT_FRONT_RIGHT = 8;
public static final int DRIVE_FRONT_RIGHT = 3;
public static final int ROT_FRONT_RIGHT = 4;

public static final int DRIVE_BACK_RIGHT = 1;
public static final int ROT_BACK_RIGHT = 2;
public static final int DRIVE_BACK_RIGHT = 5;
public static final int ROT_BACK_RIGHT = 6;

public static final int DRIVE_BACK_LEFT = 3;
public static final int ROT_BACK_LEFT = 4;
public static final int DRIVE_BACK_LEFT = 7;
public static final int ROT_BACK_LEFT = 8;

public static final int ENC_FRONT_LEFT = 11;
public static final int ENC_FRONT_RIGHT = 12;
public static final int ENC_BACK_LEFT = 10;
public static final int ENC_BACK_RIGHT = 9;
public static final int ENC_FRONT_LEFT = 9;
public static final int ENC_FRONT_RIGHT = 10;
public static final int ENC_BACK_LEFT = 11;
public static final int ENC_BACK_RIGHT = 12;

public static final int SHOOTER_LEFT = 13;
public static final int SHOOTER_RIGHT = 14;
public static final int SHOOTER_ANGLE = 15;
public static final int SHOOTER_ANGLE = 15; // make sure settings are right. something got messed up when trying to fix intake sparkmax

public static final int ELEVATOR_LEFT = 16; // TODO: Get right can ID's
public static final int ELEVATOR_RIGHT = 17;

public static final int INTAKE_ACTUATOR = 18;
public static final int INTAKE_ACTUATOR = 18; // double check this because we are switching out for a new sparkmax
public static final int INTAKE_FEED = 19;

public static final int CAN_TIMEOUT = 10;
Expand All @@ -64,10 +64,10 @@ public static class Speeds {
public static final double SHOOTER_ANGLE_MAX_SPEED = 0.3;
public static final double ELEVATOR_MAX_SPEED = 0.2;

public static final double INTAKE_FEED_SPEED = 0.25; // TODO: Get this
public static final double INTAKE_FEED_SPEED = 0.4; // TODO: Get this
public static final double OUTTAKE_FEED_SPEED = -0.4;

public static final double INTAKE_ACTUATE_MAX_SPEED = 0.5;
public static final double INTAKE_ACTUATE_MAX_SPEED = 0.3;
}

public static class Physical {
Expand All @@ -92,6 +92,8 @@ public static class Encoders {
public static final int INTAKE_OUT = 15;

public static final int SHOOTER_SHOOT_VEL = 10; // TODO: get this

public static final int ELEVATOR_MAX_HEIGHT = 155;
}

public static class FeedForward {
Expand All @@ -106,7 +108,7 @@ public static class FeedForward {
public static class PID {
// TODO: Tune everything

public static final double MODULE_DRIVE_KP = 0.05;
public static final double MODULE_DRIVE_KP = 0.015;
public static final double MODULE_ROTATION_KP = 0.009;

public static final double SHOOTER_ANGLE_KP = 0.08;
Expand Down
31 changes: 16 additions & 15 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@
public class RobotContainer {
// The robot's subsystems and commands are defined here...
private final VisionSubsystem _visionSubsystem = new VisionSubsystem();
// private final SwerveDriveSubsystem _swerveSubsystem = new SwerveDriveSubsystem(_visionSubsystem);
private final SwerveDriveSubsystem _swerveSubsystem = new SwerveDriveSubsystem(_visionSubsystem);
private final ShooterSubsystem _shooterSubsystem = new ShooterSubsystem();
private final ElevatorSubsystem _elevatorSubsystem = new ElevatorSubsystem();
private final IntakeSubsystem _intakeSubsystem = new IntakeSubsystem();
Expand Down Expand Up @@ -95,10 +95,10 @@ public RobotContainer() {
// Drive/Operate default commands


// _swerveSubsystem.setDefaultCommand(new TeleopDrive(_swerveSubsystem,
// () -> MathUtil.applyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.1),
// () -> MathUtil.applyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.1),
// () -> MathUtil.applyDeadband(-_driveFilterRightX.calculate(_driveController.getRightX()), 0.1)));
_swerveSubsystem.setDefaultCommand(new TeleopDrive(_swerveSubsystem,
() -> MathUtil.applyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.05),
() -> MathUtil.applyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.05),
() -> MathUtil.applyDeadband(-_driveFilterRightX.calculate(_driveController.getRightX()), 0.05)));

_shooterSubsystem.setDefaultCommand(new OperateShooter(
_shooterSubsystem,
Expand Down Expand Up @@ -135,10 +135,10 @@ public RobotContainer() {

// to configure button bindings
private void configureBindings() {
// Command safeActuate = new SetShooter(
// _shooterSubsystem,
// () -> 0
// ).andThen(new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.NONE));
Command safeActuate = new SetShooter(
_shooterSubsystem,
() -> 0
).andThen(new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE));

// safeActuate = new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.NONE);

Expand Down Expand Up @@ -169,10 +169,10 @@ private void configureBindings() {
// Commands.run(() -> _elevatorSubsystem.driveElevator(0.5), _elevatorSubsystem).handleInterrupt(() -> _elevatorSubsystem.stopElevator())
// );

// Runnable stop = () -> _shooterSubsystem.stopShooter();
Runnable stop = () -> _shooterSubsystem.stopShooter();

// _operatorController.L1().whileTrue(new SpinShooter(_shooterSubsystem, ShooterState.SHOOT).handleInterrupt(stop));
// _operatorController.L2().whileTrue(new SpinShooter(_shooterSubsystem, ShooterState.AMP).handleInterrupt(stop));
_operatorController.L1().whileTrue(new SpinShooter(_shooterSubsystem, ShooterState.SHOOT).handleInterrupt(stop));
_operatorController.L2().whileTrue(new SpinShooter(_shooterSubsystem, ShooterState.AMP).handleInterrupt(stop));

// _operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE));
// _operatorController.square().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE));
Expand All @@ -182,9 +182,10 @@ private void configureBindings() {
// _operatorController.square().whileTrue(
// Commands.run(() -> _intakeSubsystem.feed(FeedMode.INTAKE), _intakeSubsystem).handleInterrupt(() -> _intakeSubsystem.feed(FeedMode.NONE))
// );
_operatorController.square().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.NONE));
// _operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE));
_operatorController.triangle().whileTrue(Commands.run(_intakeSubsystem::work, _intakeSubsystem));
_operatorController.square().whileTrue(safeActuate);
_operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE));
_operatorController.cross().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.NONE, FeedMode.INTAKE));
// _operatorController.triangle().whileTrue(Commands.run(_intakeSubsystem::work, _intakeSubsystem));

// _operatorController.R1().whileTrue(new SetShooter(_shooterSubsystem, () -> 38));
// _operatorController.R2().whileTrue(new SetElevator(_elevatorSubsystem, () -> 45));
Expand Down
7 changes: 4 additions & 3 deletions src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.Constants.Encoders;
import frc.robot.utils.UtilFuncs;
import frc.robot.utils.configs.TalonFXConfig;

Expand All @@ -31,11 +32,11 @@ public ElevatorSubsystem() {

SoftwareLimitSwitchConfigs softLimits = new SoftwareLimitSwitchConfigs();

softLimits.ForwardSoftLimitThreshold = 85;
softLimits.ForwardSoftLimitThreshold = Encoders.ELEVATOR_MAX_HEIGHT;
softLimits.ReverseSoftLimitThreshold = 0;

softLimits.ForwardSoftLimitEnable = true;
softLimits.ReverseSoftLimitEnable = true;
softLimits.ForwardSoftLimitEnable = false;
softLimits.ReverseSoftLimitEnable = false;

_leftMotor.getConfigurator().apply(softLimits);

Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,8 +62,8 @@ public IntakeSubsystem() {
_actuatorMotor.setSoftLimit(SoftLimitDirection.kForward, Constants.Encoders.INTAKE_OUT);
_actuatorMotor.setSoftLimit(SoftLimitDirection.kReverse, Constants.Encoders.INTAKE_STOWED);

_actuatorMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
_actuatorMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
_actuatorMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
_actuatorMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
}

/**
Expand All @@ -85,7 +85,7 @@ public boolean noteSafety() {
}

public void work() {
_actuatorMotor.set(0.6);
_actuatorMotor.set(0.1);
}

/**
Expand Down
7 changes: 4 additions & 3 deletions src/main/java/frc/robot/subsystems/ShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ public class ShooterSubsystem extends SubsystemBase {

private final RelativeEncoder _leftEncoder = _leftMotor.getEncoder();

private final ArmFeedforward _angleFeed = new ArmFeedforward(FeedForward.SHOOTER_ANGLE_KG, 0, 0);
private final ArmFeedforward _angleFeed = new ArmFeedforward(0, 0, 0);
private final PIDController _angleController = new PIDController(PID.SHOOTER_ANGLE_KP, 0, 0);

/** Represents the state of the shooter's flywheels (speaker shoot, amp, nothing). */
Expand All @@ -54,8 +54,8 @@ public ShooterSubsystem() {
// soft limits
SoftwareLimitSwitchConfigs softLimits = new SoftwareLimitSwitchConfigs();

softLimits.ForwardSoftLimitThreshold = 54 * Constants.Physical.SHOOTER_ANGLE_GEAR_RATIO / 360;
softLimits.ReverseSoftLimitThreshold = 0 * Constants.Physical.SHOOTER_ANGLE_GEAR_RATIO / 360;
softLimits.ForwardSoftLimitThreshold = 20 * Constants.Physical.SHOOTER_ANGLE_GEAR_RATIO / 360;
softLimits.ReverseSoftLimitThreshold = -7 * Constants.Physical.SHOOTER_ANGLE_GEAR_RATIO / 360;

softLimits.ForwardSoftLimitEnable = false;
softLimits.ReverseSoftLimitEnable = false;
Expand Down Expand Up @@ -104,6 +104,7 @@ public double getVelocity() {
*/
public void driveAngle(double speed) {
_angleMotor.set(UtilFuncs.FromVolts(_angleFeed.calculate(Math.toRadians(getAngle()), 0)) + speed);
System.out.println(speed);
}

/** Stops the shooter's angular movement. */
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/utils/configs/NeoConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ public static void configureNeo(CANSparkMax neo, boolean invert) {

// neo.setCANTimeout(10);

neo.setIdleMode(IdleMode.kBrake);
neo.setIdleMode(IdleMode.kCoast);

neo.enableSoftLimit(SoftLimitDirection.kForward, false);
neo.enableSoftLimit(SoftLimitDirection.kReverse, false);
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/utils/configs/TalonFXConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,10 @@ public static TalonFXConfiguration configureFalcon(TalonFX falcon, boolean inver
config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false;
config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false;

config.CurrentLimits.StatorCurrentLimit = 50; // TODO: Find
// config.CurrentLimits.StatorCurrentLimit = 50; // TODO: Find

falcon.getConfigurator().apply(config);
falcon.setPosition(0);

return config;
}
Expand Down

0 comments on commit c27cb10

Please sign in to comment.