Skip to content
This repository has been archived by the owner on Jan 2, 2025. It is now read-only.

Commit

Permalink
Arm stinky
Browse files Browse the repository at this point in the history 
- Remove arm code
- Cleanup autonomous
  • Loading branch information
@Iooob
Iooob committed Jan 2, 2025
1 parent eec555d commit 786bb7d
Show file tree
Hide file tree
Showing 10 changed files with 19 additions and 803 deletions.
32 changes: 4 additions & 28 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,6 @@

package frc.robot;

import edu.wpi.first.math.util.Units;

/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be declared
Expand All @@ -22,13 +20,13 @@ public static final class CANConstants {
public static final int MOTORFRONTLEFTID = 12;
public static final int MOTORBACKLEFTID = 11;
/// Arm Motors
public static final int MOTORARMMAINID = 21;
public static final int MOTORARMMAINID = -1;
/// Shooter Motors
public static final int MOTORSHOOTERLEFTID = 31;
public static final int MOTORSHOOTERRIGHTID = 32;
/// Lifter Motors
public static final int MOTORLIFTERLEFTID = 41;
public static final int MOTORLIFTERRIGHTID = 42;
public static final int MOTORLIFTERLEFTID = -1;
public static final int MOTORLIFTERRIGHTID = -1;
}

public static final double MAX_SPEED = 0.8;
Expand All @@ -45,36 +43,14 @@ public static final class CANConstants {

// Joystick buttons
public static final int TRIGGER = 1;
public static final int ARMAMPBUTTON = 3;
public static final int ARMSPEAKERBUTTON = 4;
public static final int ARMDEFAULTBUTTON = 2;
public static final int ARMLOADBUTTON = 11;
public static final int ARMTRAPBUTTON = 9;
public static final int ENABLEAXISBUTTON = 10;
public static final int ARMDISABLEENCODER = 8;
public static final int AIMBUTTON = 12;

// Analog Ports
/// Ultrasonic Sensors and ports.
public static final int ULTRASONICSHOOTERPORT = 0;

// Digital Ports
public static final int ARMLIMITSWITCHFRONT = 0;
public static final int ARMLIMITSWITCHBACK = 1;
public static final int EXAMPLELIMITSWITCH = 0;

// Shooter Angles
public static final double ARMENCODEROFFSET = Units.radiansToDegrees(0.0305);
public static final double ARMSTARTINGANGLE = 22.5 + ARMENCODEROFFSET; // WHY MaTH HURT
public static final double ARMMINRELATVESTART = 0.0;
public static final double ARMLOADANGLE = 35 - ARMSTARTINGANGLE;
public static final double ARMSPEAKERANGLE = 59 - ARMSTARTINGANGLE; // to go to 75 you just put 75
public static final double ARMAMPANGLE = 110 - ARMSTARTINGANGLE;
public static final double ARMTRAPANGLE = 63 - ARMSTARTINGANGLE;
public static final double ARMMAXRELATIVE = 120 - ARMSTARTINGANGLE;
// Shooter Speeds (M/s)
public static final double SHOOTERSOURCE = -6.0;
public static final double SHOOTERAMP = 3;
public static final double SHOOTERSPEAKER = 20.0;
public static final double SHOOTERTRAP = 8.0;
public static final double SHOOTERDEFAULT = 8.0; // somewhere around 8 is the cap - ma
}
120 changes: 11 additions & 109 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,9 @@

package frc.robot;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.path.PathPlannerPath;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.GenericHID;
Expand All @@ -23,17 +21,11 @@
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.commands.AimCommand;
import frc.robot.commands.ArmCommand;
import frc.robot.commands.BalanceCommand;
import frc.robot.commands.DefaultDrive;
import frc.robot.commands.FlywheelCommand;
import frc.robot.commands.LifterCommand;
import frc.robot.commands.StraightCommand;
import frc.robot.commands.UltrasonicShooterCommand;
import frc.robot.commands.auto.AimAmpCommand;
import frc.robot.commands.auto.AimSpeakerCommand;
import frc.robot.commands.auto.LowerArmCommand;
import frc.robot.subsystems.ArmSubsystem;
import frc.robot.subsystems.CameraSubsystem;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.DriverCameraSubsystem;
Expand Down Expand Up @@ -65,7 +57,6 @@ public class RobotContainer {

private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
private final CameraSubsystem m_cameraSubsystem = new CameraSubsystem(m_driveSubsystem);
private final ArmSubsystem m_armSubsystem = new ArmSubsystem(m_shooterState);
private final FlywheelSubsystem m_shooterSubsytem = new FlywheelSubsystem();
private final LifterSubsystem m_leftLifterSubsystem =
new LifterSubsystem(Constants.CANConstants.MOTORLIFTERLEFTID);
Expand All @@ -81,48 +72,29 @@ public class RobotContainer {
new DefaultDrive(m_driveSubsystem, this::getControllerLeftY, this::getControllerRightY);
private final StraightCommand m_straightCommand =
new StraightCommand(m_driveSubsystem, this::getControllerLeftY, this::getControllerRightY);
private final UltrasonicShooterCommand m_ultrasonicShooterCommand =
new UltrasonicShooterCommand(m_ultrasonicShooterSubsystem, m_shooterState);
private final ArmCommand m_armCommand =
new ArmCommand(m_armSubsystem, m_shooterState, this::GetFlightStickY);
private final FlywheelCommand m_shooterCommand =
new FlywheelCommand(m_shooterSubsytem, m_shooterState);
private final LifterCommand m_LeftLifterCommand = new LifterCommand(m_leftLifterSubsystem);
private final LifterCommand m_RightLifterCommand = new LifterCommand(m_rightLifterSubsystem);

// these commands are used by autonomous only
private final AimAmpCommand m_AimAmpCommand = new AimAmpCommand(m_armSubsystem, m_shooterState);
private final LowerArmCommand m_AimLowerCommand =
new LowerArmCommand(m_armSubsystem, m_shooterState);
private final AimSpeakerCommand m_AimSpeakerCommand =
new AimSpeakerCommand(m_armSubsystem, m_shooterState);
// EX: these commands are used by autonomous only
// private final AimAmpCommand m_AimAmpCommand = new AimAmpCommand(m_armSubsystem,
// m_shooterState);

// this command is used by on the fly path planning
private Command m_driveToSpeaker;
private Command m_driveToAmp;
private Command m_driveToSource;
// private Command m_driveToAmp;
// Init Buttons
// private Trigger m_balanceButton;
private Trigger m_straightButton;
private Trigger m_toggleBrakeButton;
private Trigger m_lifterRightButton;
private Trigger m_lifterLeftButton;
private Trigger m_driveToSpeakerButton;
private Trigger m_driveToSourceButton;
private Trigger m_driveToAmpButton;
// private Trigger m_driveToAmpButton;
private Trigger m_lifterDirectionButton;
// joystick buttons
private JoystickButton m_aimButton;
private JoystickButton m_armRaiseToSpeakerButton;
private JoystickButton m_armRaiseToAmpButton;
private JoystickButton m_armRaiseToTrapButton;
private JoystickButton m_armWheelLoadButton;
private JoystickButton m_enableAxisButton;
private JoystickButton m_shooterTrigger;
private JoystickButton m_armRaiseToDefault;
private JoystickButton m_armDisableEncoderButton;
// Init For Autonomous
// private RamseteAutoBuilder autoBuilder;
private SendableChooser<String> autoDashboardChooser = new SendableChooser<String>();
public final boolean enableAutoProfiling = false;

Expand All @@ -136,19 +108,14 @@ public RobotContainer() {
setupTriggers();
// Bind the commands to the triggers
if (enableAutoProfiling) {
// bindDriveSysIDCommands();
bindArmSysIDCommands();
bindDriveSysIDCommands();
// bindShooterSysIDCommands();
} else {
bindCommands();
}

// set default drive command
m_driveSubsystem.setDefaultCommand(m_defaultDrive);
// set default command for shooter ultrasonic sensor
m_ultrasonicShooterSubsystem.setDefaultCommand(m_ultrasonicShooterCommand);
// set default command for arm
m_armSubsystem.setDefaultCommand(m_armCommand);
}

/**
Expand All @@ -163,20 +130,11 @@ private void setupTriggers() {
m_straightButton = m_controller1.rightBumper();
m_lifterRightButton = m_controller1.rightTrigger();
m_lifterLeftButton = m_controller1.leftTrigger();
m_driveToSpeakerButton = m_controller1.y();
m_driveToSourceButton = m_controller1.b();
// m_driveToAmpButton= m_controller1.y();
m_lifterDirectionButton = m_controller1.a();

// Joystick buttons
m_aimButton = new JoystickButton(m_flightStick, Constants.AIMBUTTON);
// arm raise buttons
m_armWheelLoadButton = new JoystickButton(m_flightStick, Constants.ARMLOADBUTTON);
m_armDisableEncoderButton = new JoystickButton(m_flightStick, Constants.ARMDISABLEENCODER);
m_armRaiseToSpeakerButton = new JoystickButton(m_flightStick, Constants.ARMSPEAKERBUTTON);
m_armRaiseToAmpButton = new JoystickButton(m_flightStick, Constants.ARMAMPBUTTON);
m_armRaiseToTrapButton = new JoystickButton(m_flightStick, Constants.ARMTRAPBUTTON);
m_enableAxisButton = new JoystickButton(m_flightStick, Constants.ENABLEAXISBUTTON);
m_armRaiseToDefault = new JoystickButton(m_flightStick, Constants.ARMDEFAULTBUTTON);

// load and shoot buttons
m_shooterTrigger = new JoystickButton(m_flightStick, Constants.TRIGGER);
Expand All @@ -187,9 +145,7 @@ private void bindCommands() {
// m_balanceButton.whileTrue(m_balanceCommand);
m_straightButton.whileTrue(m_straightCommand);
m_aimButton.whileTrue(m_aimCommand);
m_driveToSpeakerButton.whileTrue(m_driveToSpeaker);
// m_driveToAmpButton.whileTrue(m_driveToAmp); // TODO: Need to bind button
m_driveToSourceButton.whileTrue(m_driveToSource);
m_lifterRightButton.whileTrue(m_RightLifterCommand);
m_lifterLeftButton.whileTrue(m_LeftLifterCommand);
m_lifterDirectionButton.whileTrue(
Expand All @@ -198,18 +154,6 @@ private void bindCommands() {
m_toggleBrakeButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SwitchBrakemode()));
// shooter + arm commands
m_shooterTrigger.whileTrue(m_shooterCommand);
m_armDisableEncoderButton.whileTrue(new InstantCommand(() -> m_armSubsystem.disableOffset()));
m_armRaiseToSpeakerButton.whileTrue(
new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.SPEAKER)));
m_armRaiseToAmpButton.whileTrue(
new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.AMP)));
m_armWheelLoadButton.whileTrue(
new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.SOURCE)));
m_armRaiseToTrapButton.whileTrue(
new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.TRAP)));
m_armRaiseToDefault.whileTrue(
new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.DEFAULT)));
m_enableAxisButton.whileTrue(new InstantCommand(() -> m_shooterState.toggleAxis()));
}

private void bindDriveSysIDCommands() {
Expand All @@ -220,15 +164,6 @@ private void bindDriveSysIDCommands() {
m_controller1.leftTrigger().whileTrue(new InstantCommand(() -> DataLogManager.stop()));
}

private void bindArmSysIDCommands() {
m_controller1.a().whileTrue(m_armSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
m_controller1.b().whileTrue(m_armSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
m_controller1.x().whileTrue(m_armSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
m_controller1.y().whileTrue(m_armSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
m_controller1.leftTrigger().whileTrue(new InstantCommand(() -> DataLogManager.stop()));
m_armSubsystem.disablePID();
}

private void bindShooterSysIDCommands() {
m_controller1
.a()
Expand All @@ -244,10 +179,6 @@ private void bindShooterSysIDCommands() {
private void initializeAutonomous() {
// Network Table Routine Options
autoDashboardChooser.setDefaultOption("SFR", "SFR");
autoDashboardChooser.addOption("ShootSpeakerCenter", "ShootSpeakerCenter");
autoDashboardChooser.addOption("ShootSpeakerLeft", "ShootSpeakerLeft");
autoDashboardChooser.addOption("ShootSpeakerRight", "ShootSpeakerRight");
autoDashboardChooser.addOption("ShootSpeakerFar", "ShootSpeakerFar");
autoDashboardChooser.addOption("DriveForward", "DriveForward");
autoDashboardChooser.addOption("Do Nothing", "DoNothing");
SmartDashboard.putData(autoDashboardChooser);
Expand All @@ -260,47 +191,18 @@ private void initializeAutonomous() {
NamedCommands.registerCommand(
"BrakeCommand", new InstantCommand(() -> m_driveSubsystem.SetBrakemode()));
NamedCommands.registerCommand("ShooterCommand", m_shooterCommand);
NamedCommands.registerCommand("AimSpeakerCommand", m_AimSpeakerCommand);
NamedCommands.registerCommand("ArmCommand", m_armCommand);
NamedCommands.registerCommand("AimAmpCommand", m_AimAmpCommand);
NamedCommands.registerCommand("AimLowerCommand", m_AimLowerCommand);
// NamedCommands.registerCommand("AimAmpCommand", m_AimAmpCommand);

// autoBuilder =
// new RamseteAutoBuilder(
// m_driveSubsystem::getPose, // Pose2d supplier
// m_driveSubsystem
// ::resetPose, // Pose2d consumer, used to reset odometry at the beginning of auto
// new RamseteController(
// DriveConstants.kRamseteB, DriveConstants.kRamseteZeta), // RamseteController
// DriveConstants.kDriveKinematics, // DifferentialDriveKinematics
// DriveConstants
// .FeedForward, // A feedforward value to apply to the drive subsystem's
// controllers
// m_driveSubsystem::getWheelSpeeds, // A method for getting the current wheel speeds of
// the drive
// new PIDConstants(
// DriveConstants.kPDriveVel, 0, 0), // A PID controller for wheel velocity control
// m_driveSubsystem
// ::tankDriveVolts, // A consumer that takes left and right wheel voltages and sets
// // them to the drive subsystem's controllers
// autonomousEventMap,
// true, // change for either team
// m_driveSubsystem // Requirements of the commands (should be the drive subsystem)
// );
}

private void configureTeleopPaths() {
// Limits for all Paths
PathConstraints constraints =
new PathConstraints(3.0, 2.0, Units.degreesToRadians(540), Units.degreesToRadians(720));
// EX
// PathPlannerPath ampPath = PathPlannerPath.fromPathFile("TeleopAmpPath");

PathPlannerPath speakerPath = PathPlannerPath.fromPathFile("TeleopSpeakerPath");
PathPlannerPath ampPath = PathPlannerPath.fromPathFile("TeleopAmpPath");
PathPlannerPath sourcePath = PathPlannerPath.fromPathFile("TeleopSourcePath");

m_driveToSpeaker = AutoBuilder.pathfindThenFollowPath(speakerPath, constraints);
m_driveToAmp = AutoBuilder.pathfindThenFollowPath(ampPath, constraints);
m_driveToSource = AutoBuilder.pathfindThenFollowPath(sourcePath, constraints);
// m_driveToAmp = AutoBuilder.pathfindThenFollowPath(ampPath, constraints);
}

public double getControllerRightY() {
Expand Down
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/ShooterState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,8 @@ public void setLowered() {
}

public double getShooterSpeed() {
/*
Example:
switch (mode) {
case SOURCE: // TODO
return Constants.SHOOTERSOURCE;
Expand All @@ -63,6 +65,8 @@ public double getShooterSpeed() {
default:
return Constants.SHOOTERDEFAULT;
}
*/
return 0;
}

/**
Expand Down
Loading

0 comments on commit 786bb7d

Please sign in to comment.