subsystem.shooter
1from config import ShooterConfig 2import rev 3import wpilib 4from commands2 import Subsystem 5from typing import Dict 6from enum import StrEnum 7import numpy as np 8 9class ShooterMotorNames(StrEnum): 10 """ 11 Create consistent names for shooter motor references 12 """ 13 14 FEED = "feed" 15 LEAD_FLYWHEEL = "lead" 16 FOLLOWER_FLYWHEEL = "follower" 17 18 19class Shooter(Subsystem): 20 def __init__(self): 21 super().__init__() 22 self.offsetAmount = 0 23 self.RPM = 0 24 25 # Create lookup table (distance, RPM) 26 self.lookupTable = [ 27 (0.0, 1000), 28 (1.0, 1500), 29 (2.0, 2000), 30 (3.0, 3000), 31 (4.0, 3500), 32 (5.0, 4000), 33 ] 34 self.lookupTable.sort() 35 # Create an array of just distances 36 self.lookupShooterDistances = np.array([d for d, _ in self.lookupTable]) 37 # Create an array of just RPMs 38 self.lookupShooterRpms = np.array([r for _, r in self.lookupTable]) 39 40 # Instantiate motors 41 self.feedMotor = rev.SparkMax(30, rev.SparkLowLevel.MotorType.kBrushless) 42 self.leadFlywheelMotor = rev.SparkFlex(32, rev.SparkLowLevel.MotorType.kBrushless) 43 self.followerFlywheelMotor = rev.SparkFlex(33, rev.SparkLowLevel.MotorType.kBrushless) 44 45 # Set up configs for each motor 46 self.configureMotor(self.feedMotor, ShooterConfig.shooterFeedMotorPIDF, ShooterConfig.shooterInverted[0]) 47 self.configureMotor(self.leadFlywheelMotor, ShooterConfig.shooterFlywheelMotorPIDF, ShooterConfig.shooterInverted[1]) 48 self.configureMotor(self.followerFlywheelMotor, ShooterConfig.shooterFlywheelMotorPIDF, ShooterConfig.shooterInverted[2], self.leadFlywheelMotor) 49 50 self.motors: Dict[str, rev.SparkFlex | rev.SparkMax] = { 51 ShooterMotorNames.FEED: self.feedMotor, 52 ShooterMotorNames.LEAD_FLYWHEEL: self.leadFlywheelMotor, 53 ShooterMotorNames.FOLLOWER_FLYWHEEL: self.followerFlywheelMotor 54 } 55 56 # Get encoders from each motor to read data 57 self.feedEncoder = self.feedMotor.getEncoder() 58 self.leadFlywheelEncoder = self.leadFlywheelMotor.getEncoder() 59 self.followerFlywheelEncoder = self.followerFlywheelMotor.getEncoder() 60 self.encoders = { 61 ShooterMotorNames.FEED: self.feedEncoder, 62 ShooterMotorNames.LEAD_FLYWHEEL: self.leadFlywheelEncoder, 63 ShooterMotorNames.FOLLOWER_FLYWHEEL: self.followerFlywheelEncoder 64 } 65 66 # Create closed loop controllers to be able to set a reference/goal for pid 67 self.feedPID = self.feedMotor.getClosedLoopController() 68 self.leadFlywheelPID = self.leadFlywheelMotor.getClosedLoopController() 69 self.PIDs = { 70 ShooterMotorNames.FEED: self.feedPID, 71 ShooterMotorNames.LEAD_FLYWHEEL: self.leadFlywheelPID, 72 # Avoid key errors 73 ShooterMotorNames.FOLLOWER_FLYWHEEL: self.leadFlywheelPID, 74 } 75 76 def configureMotor( 77 self, motor: rev.SparkFlex | rev.SparkMax, 78 pidf: tuple, 79 invert: bool, 80 leader: rev.SparkFlex | rev.SparkMax = None 81 ): 82 """ 83 Configure the PIDF constants and inversion for the given motor. 84 85 Args: 86 motor: the motor on the shooter to configure 87 pidf: the PIDF constants to set on the given motor 88 invert: if True, invert the rotation direction of the given motor 89 leader: the motor to follow. If None, do not set this motor as a follower 90 91 Returns: 92 None 93 """ 94 configs = rev.SparkBaseConfig() 95 96 if leader is not None: 97 configs.follow(leader=leader, invert=invert) 98 else: 99 configs.inverted(invert) 100 configs.closedLoop.pidf(*pidf, rev.ClosedLoopSlot.kSlot0) 101 102 motor.configure(configs, rev.ResetMode.kResetSafeParameters, rev.PersistMode.kPersistParameters) 103 104 def setMotorVoltage(self, motorName: str, voltage: float): 105 """ 106 Sets the voltage of the motor 107 108 Args: 109 motorName: Name of the motor to set a voltage for (ie: 'feed') 110 voltage: Voltage to set the motor at 111 112 Returns: 113 None 114 """ 115 self.motors[motorName].setVoltage(voltage) 116 117 def setMotorReference(self, motorName: str, rpm: float): 118 """ 119 Give a custom setpoint for PID to achieve in terms of velocity 120 121 Args: 122 motorName: Name of the motor 123 rpm: The velocity setpoint for the motor in RPM 124 125 Returns: 126 None 127 """ 128 self.PIDs[motorName].setReference(rpm, rev.SparkLowLevel.ControlType.kVelocity, rev.ClosedLoopSlot.kSlot0) 129 130 def setRPM(self, rpm: float): 131 """ 132 Directly set the RPM using the given value 133 134 Args: 135 rpm: The velocity setpoint for the motor in RPM 136 137 Returns: 138 None 139 """ 140 self.RPM = rpm 141 142 def getVelocity(self, motorName: str): 143 """ 144 Get the current velocity of a motor in RPM 145 146 Args: 147 motorName: Name of the motor 148 149 Returns: 150 Velocity of the motor in RPM 151 """ 152 return self.encoders[motorName].getVelocity() 153 154 def setRpmUsingLookup(self, distance: float): 155 """ 156 Set the RPM needed to shoot the ball at a specified distance 157 158 Args: 159 distance: distance in meters from a target point 160 161 Returns: 162 None 163 """ 164 # Get RPM from the distance given 165 self.RPM = float(np.interp(distance, self.lookupShooterDistances, self.lookupShooterRpms)) 166 167 def modifyOffset(self, offsetDelta: float): 168 """ 169 Modify the RPM offset 170 171 Args: 172 offsetDelta: change in offset that is applied 173 174 Returns: 175 None 176 """ 177 self.offsetAmount = self.offsetAmount + offsetDelta 178 179 def resetOffset(self): 180 """ 181 Reset the RPM offset 182 183 Args: 184 None 185 186 Returns: 187 None 188 """ 189 self.offsetAmount = 0 190 191 def getOffset(self): 192 """ 193 Get the RPM offset 194 195 Args: 196 None 197 198 Returns: 199 None 200 """ 201 return self.offsetAmount 202 203 def periodic(self): 204 newRPM = self.RPM + self.offsetAmount 205 feedRPM = int(newRPM * ShooterConfig.shooterFeedPercentOfFlywheel) 206 207 self.setMotorReference(ShooterMotorNames.FEED, feedRPM) 208 self.setMotorReference(ShooterMotorNames.LEAD_FLYWHEEL, newRPM) 209 210 wpilib.SmartDashboard.putNumber("Shooter_RPM", newRPM) 211 wpilib.SmartDashboard.putNumber("Shooter_Feed_RPM", feedRPM) 212 wpilib.SmartDashboard.putNumber("Shooter_Offset", self.offsetAmount)
10class ShooterMotorNames(StrEnum): 11 """ 12 Create consistent names for shooter motor references 13 """ 14 15 FEED = "feed" 16 LEAD_FLYWHEEL = "lead" 17 FOLLOWER_FLYWHEEL = "follower"
Create consistent names for shooter motor references
20class Shooter(Subsystem): 21 def __init__(self): 22 super().__init__() 23 self.offsetAmount = 0 24 self.RPM = 0 25 26 # Create lookup table (distance, RPM) 27 self.lookupTable = [ 28 (0.0, 1000), 29 (1.0, 1500), 30 (2.0, 2000), 31 (3.0, 3000), 32 (4.0, 3500), 33 (5.0, 4000), 34 ] 35 self.lookupTable.sort() 36 # Create an array of just distances 37 self.lookupShooterDistances = np.array([d for d, _ in self.lookupTable]) 38 # Create an array of just RPMs 39 self.lookupShooterRpms = np.array([r for _, r in self.lookupTable]) 40 41 # Instantiate motors 42 self.feedMotor = rev.SparkMax(30, rev.SparkLowLevel.MotorType.kBrushless) 43 self.leadFlywheelMotor = rev.SparkFlex(32, rev.SparkLowLevel.MotorType.kBrushless) 44 self.followerFlywheelMotor = rev.SparkFlex(33, rev.SparkLowLevel.MotorType.kBrushless) 45 46 # Set up configs for each motor 47 self.configureMotor(self.feedMotor, ShooterConfig.shooterFeedMotorPIDF, ShooterConfig.shooterInverted[0]) 48 self.configureMotor(self.leadFlywheelMotor, ShooterConfig.shooterFlywheelMotorPIDF, ShooterConfig.shooterInverted[1]) 49 self.configureMotor(self.followerFlywheelMotor, ShooterConfig.shooterFlywheelMotorPIDF, ShooterConfig.shooterInverted[2], self.leadFlywheelMotor) 50 51 self.motors: Dict[str, rev.SparkFlex | rev.SparkMax] = { 52 ShooterMotorNames.FEED: self.feedMotor, 53 ShooterMotorNames.LEAD_FLYWHEEL: self.leadFlywheelMotor, 54 ShooterMotorNames.FOLLOWER_FLYWHEEL: self.followerFlywheelMotor 55 } 56 57 # Get encoders from each motor to read data 58 self.feedEncoder = self.feedMotor.getEncoder() 59 self.leadFlywheelEncoder = self.leadFlywheelMotor.getEncoder() 60 self.followerFlywheelEncoder = self.followerFlywheelMotor.getEncoder() 61 self.encoders = { 62 ShooterMotorNames.FEED: self.feedEncoder, 63 ShooterMotorNames.LEAD_FLYWHEEL: self.leadFlywheelEncoder, 64 ShooterMotorNames.FOLLOWER_FLYWHEEL: self.followerFlywheelEncoder 65 } 66 67 # Create closed loop controllers to be able to set a reference/goal for pid 68 self.feedPID = self.feedMotor.getClosedLoopController() 69 self.leadFlywheelPID = self.leadFlywheelMotor.getClosedLoopController() 70 self.PIDs = { 71 ShooterMotorNames.FEED: self.feedPID, 72 ShooterMotorNames.LEAD_FLYWHEEL: self.leadFlywheelPID, 73 # Avoid key errors 74 ShooterMotorNames.FOLLOWER_FLYWHEEL: self.leadFlywheelPID, 75 } 76 77 def configureMotor( 78 self, motor: rev.SparkFlex | rev.SparkMax, 79 pidf: tuple, 80 invert: bool, 81 leader: rev.SparkFlex | rev.SparkMax = None 82 ): 83 """ 84 Configure the PIDF constants and inversion for the given motor. 85 86 Args: 87 motor: the motor on the shooter to configure 88 pidf: the PIDF constants to set on the given motor 89 invert: if True, invert the rotation direction of the given motor 90 leader: the motor to follow. If None, do not set this motor as a follower 91 92 Returns: 93 None 94 """ 95 configs = rev.SparkBaseConfig() 96 97 if leader is not None: 98 configs.follow(leader=leader, invert=invert) 99 else: 100 configs.inverted(invert) 101 configs.closedLoop.pidf(*pidf, rev.ClosedLoopSlot.kSlot0) 102 103 motor.configure(configs, rev.ResetMode.kResetSafeParameters, rev.PersistMode.kPersistParameters) 104 105 def setMotorVoltage(self, motorName: str, voltage: float): 106 """ 107 Sets the voltage of the motor 108 109 Args: 110 motorName: Name of the motor to set a voltage for (ie: 'feed') 111 voltage: Voltage to set the motor at 112 113 Returns: 114 None 115 """ 116 self.motors[motorName].setVoltage(voltage) 117 118 def setMotorReference(self, motorName: str, rpm: float): 119 """ 120 Give a custom setpoint for PID to achieve in terms of velocity 121 122 Args: 123 motorName: Name of the motor 124 rpm: The velocity setpoint for the motor in RPM 125 126 Returns: 127 None 128 """ 129 self.PIDs[motorName].setReference(rpm, rev.SparkLowLevel.ControlType.kVelocity, rev.ClosedLoopSlot.kSlot0) 130 131 def setRPM(self, rpm: float): 132 """ 133 Directly set the RPM using the given value 134 135 Args: 136 rpm: The velocity setpoint for the motor in RPM 137 138 Returns: 139 None 140 """ 141 self.RPM = rpm 142 143 def getVelocity(self, motorName: str): 144 """ 145 Get the current velocity of a motor in RPM 146 147 Args: 148 motorName: Name of the motor 149 150 Returns: 151 Velocity of the motor in RPM 152 """ 153 return self.encoders[motorName].getVelocity() 154 155 def setRpmUsingLookup(self, distance: float): 156 """ 157 Set the RPM needed to shoot the ball at a specified distance 158 159 Args: 160 distance: distance in meters from a target point 161 162 Returns: 163 None 164 """ 165 # Get RPM from the distance given 166 self.RPM = float(np.interp(distance, self.lookupShooterDistances, self.lookupShooterRpms)) 167 168 def modifyOffset(self, offsetDelta: float): 169 """ 170 Modify the RPM offset 171 172 Args: 173 offsetDelta: change in offset that is applied 174 175 Returns: 176 None 177 """ 178 self.offsetAmount = self.offsetAmount + offsetDelta 179 180 def resetOffset(self): 181 """ 182 Reset the RPM offset 183 184 Args: 185 None 186 187 Returns: 188 None 189 """ 190 self.offsetAmount = 0 191 192 def getOffset(self): 193 """ 194 Get the RPM offset 195 196 Args: 197 None 198 199 Returns: 200 None 201 """ 202 return self.offsetAmount 203 204 def periodic(self): 205 newRPM = self.RPM + self.offsetAmount 206 feedRPM = int(newRPM * ShooterConfig.shooterFeedPercentOfFlywheel) 207 208 self.setMotorReference(ShooterMotorNames.FEED, feedRPM) 209 self.setMotorReference(ShooterMotorNames.LEAD_FLYWHEEL, newRPM) 210 211 wpilib.SmartDashboard.putNumber("Shooter_RPM", newRPM) 212 wpilib.SmartDashboard.putNumber("Shooter_Feed_RPM", feedRPM) 213 wpilib.SmartDashboard.putNumber("Shooter_Offset", self.offsetAmount)
A robot subsystem. Subsystems are the basic unit of robot organization in the Command-based
framework; they encapsulate low-level hardware objects (motor controllers, sensors, etc.) and
provide methods through which they can be used by Commands. Subsystems are used by the
CommandScheduler's resource management system to ensure multiple robot actions are not
"fighting" over the same hardware; Commands that use a subsystem should include that subsystem in
their commands2.Command.getRequirements() method, and resources used within a subsystem should
generally remain encapsulated and not be shared by other parts of the robot.
Subsystems must be registered with the scheduler with the commands2.CommandScheduler.registerSubsystem()
method in order for the .periodic() method to be called. It is recommended that this method be called from the
constructor of users' Subsystem implementations.
21 def __init__(self): 22 super().__init__() 23 self.offsetAmount = 0 24 self.RPM = 0 25 26 # Create lookup table (distance, RPM) 27 self.lookupTable = [ 28 (0.0, 1000), 29 (1.0, 1500), 30 (2.0, 2000), 31 (3.0, 3000), 32 (4.0, 3500), 33 (5.0, 4000), 34 ] 35 self.lookupTable.sort() 36 # Create an array of just distances 37 self.lookupShooterDistances = np.array([d for d, _ in self.lookupTable]) 38 # Create an array of just RPMs 39 self.lookupShooterRpms = np.array([r for _, r in self.lookupTable]) 40 41 # Instantiate motors 42 self.feedMotor = rev.SparkMax(30, rev.SparkLowLevel.MotorType.kBrushless) 43 self.leadFlywheelMotor = rev.SparkFlex(32, rev.SparkLowLevel.MotorType.kBrushless) 44 self.followerFlywheelMotor = rev.SparkFlex(33, rev.SparkLowLevel.MotorType.kBrushless) 45 46 # Set up configs for each motor 47 self.configureMotor(self.feedMotor, ShooterConfig.shooterFeedMotorPIDF, ShooterConfig.shooterInverted[0]) 48 self.configureMotor(self.leadFlywheelMotor, ShooterConfig.shooterFlywheelMotorPIDF, ShooterConfig.shooterInverted[1]) 49 self.configureMotor(self.followerFlywheelMotor, ShooterConfig.shooterFlywheelMotorPIDF, ShooterConfig.shooterInverted[2], self.leadFlywheelMotor) 50 51 self.motors: Dict[str, rev.SparkFlex | rev.SparkMax] = { 52 ShooterMotorNames.FEED: self.feedMotor, 53 ShooterMotorNames.LEAD_FLYWHEEL: self.leadFlywheelMotor, 54 ShooterMotorNames.FOLLOWER_FLYWHEEL: self.followerFlywheelMotor 55 } 56 57 # Get encoders from each motor to read data 58 self.feedEncoder = self.feedMotor.getEncoder() 59 self.leadFlywheelEncoder = self.leadFlywheelMotor.getEncoder() 60 self.followerFlywheelEncoder = self.followerFlywheelMotor.getEncoder() 61 self.encoders = { 62 ShooterMotorNames.FEED: self.feedEncoder, 63 ShooterMotorNames.LEAD_FLYWHEEL: self.leadFlywheelEncoder, 64 ShooterMotorNames.FOLLOWER_FLYWHEEL: self.followerFlywheelEncoder 65 } 66 67 # Create closed loop controllers to be able to set a reference/goal for pid 68 self.feedPID = self.feedMotor.getClosedLoopController() 69 self.leadFlywheelPID = self.leadFlywheelMotor.getClosedLoopController() 70 self.PIDs = { 71 ShooterMotorNames.FEED: self.feedPID, 72 ShooterMotorNames.LEAD_FLYWHEEL: self.leadFlywheelPID, 73 # Avoid key errors 74 ShooterMotorNames.FOLLOWER_FLYWHEEL: self.leadFlywheelPID, 75 }
__init__(self: wpiutil._wpiutil.Sendable) -> None
77 def configureMotor( 78 self, motor: rev.SparkFlex | rev.SparkMax, 79 pidf: tuple, 80 invert: bool, 81 leader: rev.SparkFlex | rev.SparkMax = None 82 ): 83 """ 84 Configure the PIDF constants and inversion for the given motor. 85 86 Args: 87 motor: the motor on the shooter to configure 88 pidf: the PIDF constants to set on the given motor 89 invert: if True, invert the rotation direction of the given motor 90 leader: the motor to follow. If None, do not set this motor as a follower 91 92 Returns: 93 None 94 """ 95 configs = rev.SparkBaseConfig() 96 97 if leader is not None: 98 configs.follow(leader=leader, invert=invert) 99 else: 100 configs.inverted(invert) 101 configs.closedLoop.pidf(*pidf, rev.ClosedLoopSlot.kSlot0) 102 103 motor.configure(configs, rev.ResetMode.kResetSafeParameters, rev.PersistMode.kPersistParameters)
Configure the PIDF constants and inversion for the given motor.
Args: motor: the motor on the shooter to configure pidf: the PIDF constants to set on the given motor invert: if True, invert the rotation direction of the given motor leader: the motor to follow. If None, do not set this motor as a follower
Returns: None
105 def setMotorVoltage(self, motorName: str, voltage: float): 106 """ 107 Sets the voltage of the motor 108 109 Args: 110 motorName: Name of the motor to set a voltage for (ie: 'feed') 111 voltage: Voltage to set the motor at 112 113 Returns: 114 None 115 """ 116 self.motors[motorName].setVoltage(voltage)
Sets the voltage of the motor
Args: motorName: Name of the motor to set a voltage for (ie: 'feed') voltage: Voltage to set the motor at
Returns: None
118 def setMotorReference(self, motorName: str, rpm: float): 119 """ 120 Give a custom setpoint for PID to achieve in terms of velocity 121 122 Args: 123 motorName: Name of the motor 124 rpm: The velocity setpoint for the motor in RPM 125 126 Returns: 127 None 128 """ 129 self.PIDs[motorName].setReference(rpm, rev.SparkLowLevel.ControlType.kVelocity, rev.ClosedLoopSlot.kSlot0)
Give a custom setpoint for PID to achieve in terms of velocity
Args: motorName: Name of the motor rpm: The velocity setpoint for the motor in RPM
Returns: None
131 def setRPM(self, rpm: float): 132 """ 133 Directly set the RPM using the given value 134 135 Args: 136 rpm: The velocity setpoint for the motor in RPM 137 138 Returns: 139 None 140 """ 141 self.RPM = rpm
Directly set the RPM using the given value
Args: rpm: The velocity setpoint for the motor in RPM
Returns: None
143 def getVelocity(self, motorName: str): 144 """ 145 Get the current velocity of a motor in RPM 146 147 Args: 148 motorName: Name of the motor 149 150 Returns: 151 Velocity of the motor in RPM 152 """ 153 return self.encoders[motorName].getVelocity()
Get the current velocity of a motor in RPM
Args: motorName: Name of the motor
Returns: Velocity of the motor in RPM
155 def setRpmUsingLookup(self, distance: float): 156 """ 157 Set the RPM needed to shoot the ball at a specified distance 158 159 Args: 160 distance: distance in meters from a target point 161 162 Returns: 163 None 164 """ 165 # Get RPM from the distance given 166 self.RPM = float(np.interp(distance, self.lookupShooterDistances, self.lookupShooterRpms))
Set the RPM needed to shoot the ball at a specified distance
Args: distance: distance in meters from a target point
Returns: None
168 def modifyOffset(self, offsetDelta: float): 169 """ 170 Modify the RPM offset 171 172 Args: 173 offsetDelta: change in offset that is applied 174 175 Returns: 176 None 177 """ 178 self.offsetAmount = self.offsetAmount + offsetDelta
Modify the RPM offset
Args: offsetDelta: change in offset that is applied
Returns: None
180 def resetOffset(self): 181 """ 182 Reset the RPM offset 183 184 Args: 185 None 186 187 Returns: 188 None 189 """ 190 self.offsetAmount = 0
Reset the RPM offset
Args: None
Returns: None
192 def getOffset(self): 193 """ 194 Get the RPM offset 195 196 Args: 197 None 198 199 Returns: 200 None 201 """ 202 return self.offsetAmount
Get the RPM offset
Args: None
Returns: None
204 def periodic(self): 205 newRPM = self.RPM + self.offsetAmount 206 feedRPM = int(newRPM * ShooterConfig.shooterFeedPercentOfFlywheel) 207 208 self.setMotorReference(ShooterMotorNames.FEED, feedRPM) 209 self.setMotorReference(ShooterMotorNames.LEAD_FLYWHEEL, newRPM) 210 211 wpilib.SmartDashboard.putNumber("Shooter_RPM", newRPM) 212 wpilib.SmartDashboard.putNumber("Shooter_Feed_RPM", feedRPM) 213 wpilib.SmartDashboard.putNumber("Shooter_Offset", self.offsetAmount)
This method is called periodically by the CommandScheduler. Useful for updating subsystem-specific state that you don't want to offload to a Command. Teams should try to be consistent within their own codebases about which responsibilities will be handled by Commands, and which will be handled here.