Skip to content

Subsystems and code structure

Jump to bottom
AlexHuie edited this page Feb 15, 2024 · 15 revisions

The robot has 6 main subsystems which control its various actions. These are:

  • Driving: SwerveSubsystem
  • Intake: IntakeSubsytem
  • Shooter/Indexer: ShooterSubsystem
  • Arm: ArmSubsystem
  • Climbers: ClimberSubsystem
  • LEDs: LEDController

Additionally, the Vision class contains static methods for getting data from the two limelights, and the SwerveModule class contains data about a swerve module and methods to set and get its state.

Each subsystem has a corresponding static subclass in the Constants file where its CAN IDs, control constants, distances, etc. are stored.

Drive Subsystem

Member Variables

  • private SwerveModule[] swerveModules - an array of all four SwerveModules. #0 is the front left wheel, and we count up counterclockwise

  • private SwerveDrivePoseEstimator odometry - WPIlib pose estimator which combines wheel measurements with vision measurements to estimate the robot's location on the field

  • private Field2d field - used to display the robot's pose estimate on Shuffleboard

  • private final Pigeon2 pigeon - CTRE Pigeon sensor which measures changes in the robot's direction

Private Methods

  • SwerveModulePosition[] getPositions(): gets the SwerveModulePosition (distance traveled and current direction) for each module and returns them in an array

  • SwerveModuleState[] getStates(): gets the SwerveModuleState (speed and direction) for each module and returns them in an array

Public Methods

  • void drive() - drive function used during teleop control. Sets the robot to drive at a given speed, and uses open loop control to set the modules' speeds

    • Inputs:
      • double x, double y: the speed to move the robot on the x and y axes in meters/second
      • double rotation: the speed to spin the robot in radians/second
      • boolean isFieldRelative: if set to true, the robot will use the field's coordinate system instead of its own to determine driving speeds

  • void closedLoopDrive() - drive function used by the PathPlanner library while following trajectories. Sets the robot to drive at a given ChassisSpeeds and uses closed loop control to set the modules' speeds

  • void driveFromChassisSpeeds() - the core drive function of the robot, used by both other drive functions and during auto-aligning. Calculates the correct speed and direction for each wheel and passes them to the four modules.

    • Inputs:
      • ChassisSpeeds speeds: the speed to move the robot (ChassisSpeeds contains both translation and rotational speeds)
      • boolean isOpenLoop: whether the modules should control their speed with open- or closed-loop control

  • void resetOdometry(): sets the robot's pose to a given Pose2d

  • void zeroGyro(): sets the pigeon's "zero" direction to be the direction the robot is currently facing

  • Pose2d getPose(): returns the robots pose in meters as estimated by the odometry

  • ChassisSpeeds getRobotRelativeSpeeds() : gets the robot's current velocity and angular velocity of the robot in meters/second and radians/second. Returns the data as a ChassisSpeeds

  • double getYawAsDouble() : gets the current heading measurement in degrees from the pigeon2. This value is not limited to 0-360, so if you spin around twice, the pigeon would read 720 degrees

  • Rotation2d getYaw() : constructs a Rotation2d with the heading measurement from the pigeon2

  • void periodic() : this function gets called every 20ms while the robot is powered (even if it's disabled). Anything that needs to be constantly updated goes here:

    • if an AprilTag is in view of the limelight, update the odometry with the limelight's estimated pose

    • supply info about the drivebase and swerve modules to the driverstation

Arm Subsystem

Member Variables

  • the arm is actuated by two Falcon500 motors. They operate in sync, but rotate in opposite directions.
    • private final TalonFX motorR - The leader motor, configured to run a MotionMagic control loop
    • private final TalonFX motorL - the follower motor, configured to copy motorR's motion, but in the opposite direction
  • private MotionMagicConfig MMconfig - configures the maximum velocity and acceleration for the motion profile generation
  • private SoftwareLimitSwitchConfigs limitConfig - configures the arms minimum and maximum positions
  • private Slot0Config PIDConfig - configures PID and Feedforward control loop. The constants themselves are in Constants.ArmConstants
  • private double setpoint - the goal position of the arm

Public Methods

  • void setTarget(): sets the goal angle of the arm to a number of degrees. 0 is horizontal.
  • double getArmPosition(): gets the current angle of the arm in degrees
  • boolean isAtTarget(): returns true if the arm is at the goal position (plus or minus a small threshold value)

Intake Subsystem

Member Variables

  • private final CANSparkMax intakeMotor: The motor that runs the intake
  • private final DigitalInput beamSensor: unused

Public Methods

  • void runIntake(double speed): Runs the intake at currently undetermined speed
  • boolean isNoteInIntake(): Checks whether or not the note is in the intake

Climber Subsystem

Member Variables

  • There are two motors on the climber
  • private final CANSparkMax motorL: Left motor for the climber
  • private final CANSparkMax motorR: Right motor for the climber

Public Methods

  • Command climb(DoubleSupplier leftSpeed, DoubleSupplier rightSpeed): Sets to an undetermined speed

LED Controller

Member Variables

  • private AddressableLED leds: The strip of leds on the robot
  • private AddressableLEDBuffer buffer:
  • private LedPattern currentPattern: This variable controls the pattern displayed by the leds

Shooter Subsystem

Member Variables

  • private final CANSparkMax beltMotor: The motor that controls the belt
  • private final CANSparkMax shooterMotor: The motor that controls the shooter
  • private final DigitalInput beamSensor: unused
  • private final RelativeEncoder shooterEncoder: An encoder for the shooterMotor position
  • private final SparkPIDController shooterSpeedController: Allows us to use PID to control the shooterMotor
  • private final SimpleMotorFeedForward shooterFeedforward: A variable that accounts for the force of gravity
  • private double setpoint: Undefined set point

Public Methods

  • boolean isNoteInShooter(): Checking if the note is in the shooter
  • double getShooterWheelSpeed(): Getting the shooter wheel speed through the shooter encoder
  • boolean isAtShootingSpeed(): Checking if the shooter is at the shooter speed
  • void setBeltSpeed(double speed): Setting the belt speed to a currently undetermined speed
  • void setShooterSpeed(double speed): Setting the shooter speed to a currently undetermined speed
  • void setTargetShooterSpeed(double rotationsPerSec): Setting the target shooter speed using PID velocity control
Clone this wiki locally