subsystem.intakeactions
1import wpilib 2import commands2 3import rev 4import time 5# import array as arr 6 7from constants import CaptainPlanetConsts as intakeConsts 8# from config import OperatorRobotConfig 9 10intakeVelocity = 0.3 #Speed (in rpm) in which the intake motor will move upon deployment/stowing 11intakeMotorThreshold = 1 #Used to determine whether or not intake is deployed 12intakeFaultThreshold = 10 #Amount of time spent trying to deploy/stow intake before fault condition is triggered 13rollerVelocity = 0.3 #Speed in which the roller motor will move upon deployment 14rollerFaultThreshold = 10 #Amount of time spent trying to operate rollers before fault condition is triggered 15jamFaultThreshold = 10 #Amount of attempts done trying to reverse rollers in the event of a jam before a fault condition is triggered 16 17baselineFault = 0 #Leave at 0, provides baseline to compare to when determining faults 18baselineJam = 0 #Leave at 0, provides baseline to compare to when determining faults 19jamReversalCount = 0 #Leave at 0, stores amount of attempts in reversing motors in the event of a jam before a fault condition is triggered 20 21class IntakeSubsystem(commands2.SubsystemBase): 22 def __init__(self, alternateConfiguration = False): 23 #Initializes all devices 24 #Alternate Configuration effectively just switches the motors used for intake deployment and rollers 25 if alternateConfiguration == True: 26 # self.intakeMotor = rev.SparkMax(intakeConsts.kRollerMotorCanId, rev.SparkLowLevel.MotorType.kBrushless) 27 # self.intakeMotorEncoder = self.intakeMotor.getEncoder() 28 # self.intakeMotorConfig = rev.SparkMaxConfig() 29 # self.tuningMotors() 30 #self.intakeMotorPosition = self.intakeMotorEncoder.getPosition() 31 32 self.rollerMotor = rev.SparkFlex(intakeConsts.kIntakeMotorCanId, rev.SparkLowLevel.MotorType.kBrushless) 33 self.rollerMotorEncoder = self.rollerMotor.getEncoder() 34 self.rollerMotorVelocity = self.rollerMotorEncoder.getVelocity() 35 else: 36 # self.intakeMotor = rev.SparkFlex(intakeConsts.kIntakeMotorCanId, rev.SparkLowLevel.MotorType.kBrushless) 37 # self.intakeMotorEncoder = self.intakeMotor.getEncoder() 38 #self.intakeMotorPosition = self.intakeMotorEncoder.getPosition() 39 40 self.rollerMotor = rev.SparkMax(intakeConsts.kRollerMotorCanId, rev.SparkLowLevel.MotorType.kBrushless) 41 self.rollerMotorEncoder = self.rollerMotor.getEncoder() 42 self.rollerMotorVelocity = self.rollerMotorEncoder.getVelocity() 43 44 # self.breakBeam = wpilib.DigitalInput(intakeConsts.kBreakBeam) 45 # self.frontBreakbeam = wpilib.DigitalInput(intakeConsts.kFrontBreakBeam) 46 # self.backBreakbeam = wpilib.DigitalInput(intakeConsts.kBackBreakBeam) 47 # self.frontBeamBroken = not self.frontBreakbeam.get() 48 # self.backBeamBroken = not self.backBreakbeam.get() 49 50 # self.HallEffectSensor = wpilib.DigitalInput(intakeConsts.kHallEffectSensor) 51 52 #Set Variables 53 # self.intakeDeployed = 200 #Minimum amount of rotations before assuming intake is deployed 54 # self.intakeStowed = 0 #Maximum amount of rotations before assuming intake is stowed 55 # self.intakeFaultThreshold = 2 #Amount of time spent trying to deploy/stow intake before fault condition is triggered 56 # self.intakeMagnetFaultThreshold = 2 #Amount of time before magnets need to have stopped tripping hall effects sensor or fault condition is triggered 57 self.rollerFaultThreshold = 2 #Amount of time spent trying to operate rollers before fault condition is triggered 58 self.jamFaultThreshold = 0 #Amount of attempts done trying to reverse rollers in the event of a jam before a fault condition is triggered 59 self.jamTime = 1 #Amount of time to wait before assuming a ball inside the intake has gotten stuck 60 self.jamThreshold = 10 #Maximum sustained rpm before assuming a ball inside the rollers has gotten stuck 61 self.jamReversalTime = 3 #Amount of time to have motors reverse when a ball inside the intake has gotten stuck 62 self.unjam = 1500 #Minimum sustained rpm before assuming rollers have been unjammed 63 64 # self.intakeCondition = 0 #Leave at 0, provides reference to code on current intake status 65 # self.intakeRamped = 0 #Leave at 0, provides reference to code on ramping intake status 66 # self.intakeRampedCondition = 0 #Leave at 0, provides reference to code on whether ramping intake is finished 67 # self.intakeVelocity = 0 #Leave at 0, any updating is to be done thru Network Table, Speed (in rpm) in which the intake motor will move upon deployment/stowing 68 self.rollerVelocity = 0 #Leave at 0, any updating is to be done thru Network Table, Speed in which the roller motor will move upon deployment 69 self.baselineFault = 0 #Leave at 0, provides baseline to compare to when determining faults 70 self.baselineJam = 0 #Leave at 0, provides baseline to compare to when determining faults 71 self.jamReversalCount = 0 #Leave at 0, stores amount of attempts in reversing motors in the event of a jam before a fault condition is triggered 72 # self.intakeDifference = 0 #Leave at 0, rotations required to get from intake stowed position to intake deployed position is automatically calculated 73 # self.remainingRotations = 0 #Leave at 0, rotations remaining to finish deploying/stowing intake is automatically calculated 74 # self.intakeSlowdownPosition = 0 #Leave at 0, stores amount of intake motor rotations required to slow it down 75 # self.intakeRamp = 0 #Leave at 0, motor position for ramp is automatically calculated 76 # self.intakeRampStatus = 0 #Leave at 0, provides reference to code on whether intake is moving to ramp 77 # self.hardStopIndex = 0 #Leave at 0, provides index to code for hardstop checks 78 self.jamOccurence = 0 #Leave at 0, provides baseline to compare to when determining jams 79 self.baselineDetectedJam = 0 #Leave at 0, provides baseline to compare to when jam detection is activated 80 self.rollerCondition = 0 #Leave at 0, provides reference to code on current roller status 81 self.rollerSensor = 0 #Leave at 0, ensures that the rollers are stopped only once, preventing obstruction of manual controls 82 83 self.jamDetected = False #Leave at False 84 # self.intakeMotorPositions = arr.array('f', [0,0,0,0,0]) #Leave with all zeros, for checking if intake motor stopped during deployment/stowing 85 86 # def deployIntake(self): 87 # #Check Sensor for deployment, if not, deploy it. 88 # if self.intakeCondition <= 0 and self.intakeMotorEncoder.getPosition() <= self.intakeDeployed: 89 # self.baselineFault = time.perf_counter() 90 # self.intakeCondition = 1 91 # if self.intakeCondition >= 0: 92 # if self.HallEffectSensor.get() == False: 93 # self.intakeDeployed = self.intakeMotorEncoder.getPosition() 94 # self.intakeCondition = 0 95 # if self.intakeMotorEncoder.getPosition() >= self.intakeDeployed: 96 # self.intakeCondition = 0 97 # if self.baselineFault - time.perf_counter() >= self.intakeFaultThreshold: 98 # wpilib.Alert("INTAKE ERR101: Deployment of intake dosen't appear to be working! Stopped activation.", wpilib.Alert.AlertType.kError) 99 # return 100 # else: 101 # self.intakeCondition = 0 102 103 def activateRoller(self): 104 # self.baselineFault = time.perf_counter() 105 106 # Apply voltage to roller until it starts moving; Terminate program with ERR103 if fault condition is detected 107 # while self.rollerMotorEncoder.getVelocity() == 0: 108 if self.rollerCondition != 1: 109 self.rollerCondition = 1 110 # if baselineFault - time.perf_counter() >= self.rollerFaultThreshold: 111 # wpilib.Alert("INTAKE ERR103: Activation of rollers don't appear to be working! Stopped activation.", wpilib.Alert.AlertType.kError) 112 # return 113 114 def deactivateRoller(self): 115 # self.baselineFault = time.perf_counter() 116 117 # Try to terminate voltage until motor stops moving; Terminate program with ERR103 if fault condition is detected 118 # while self.rollerMotorEncoder.getVelocity() != 0: 119 if self.rollerCondition != 0: 120 self.rollerCondition = 0 121 # if baselineFault - time.perf_counter() >= self.rollerFaultThreshold: 122 # wpilib.Alert("INTAKE ERR103: Activation of rollers don't appear to be working! Stopped activation.", wpilib.Alert.AlertType.kError) 123 # return 124 125 # def stowIntake(self): 126 # if self.intakeCondition >= 0 and self.intakeMotorEncoder.getPosition() >= self.intakeStowed: 127 # self.baselineFault = time.perf_counter() 128 # self.intakeCondition = -1 129 # if self.intakeCondition <= 0: 130 # if self.intakeMotorEncoder.getPosition() <= self.intakeStowed: 131 # self.intakeCondition = 0 132 # if self.baselineFault - time.perf_counter() >= self.intakeFaultThreshold: 133 # wpilib.Alert("INTAKE ERR112: Intake Stow doesn't appear to be working! Stopping activation.", wpilib.Alert.AlertType.kError) 134 # return 135 # if self.intakeMagnetFaultThreshold + 1 >= time.perf_counter() - self.baselineFault >= self.intakeMagnetFaultThreshold: 136 # if self.HallEffectSensor.get() == False: 137 # wpilib.Alert("INTAKE ERR112: Intake motor is engaged but the Intake doesn't appear to be moving! Stopping code.", wpilib.Alert.AlertType.kError) 138 # return 139 # else: 140 # self.intakeCondition = 0 141 142 def jamDetection(self): 143 if not self.jamDetected: 144 if self.rollerMotorEncoder.getVelocity() <= self.jamThreshold and self.rollerCondition == 1: 145 if self.jamOccurence == 0: 146 self.baselineJam = time.perf_counter() 147 self.jamOccurence = 1 148 else: 149 if time.perf_counter() - self.baselineJam >= self.jamTime: 150 self.baselineDetectedJam = time.perf_counter() 151 self.jamDetected = True 152 else: 153 if time.perf_counter() - self.baselineDetectedJam <= self.jamReversalTime and self.jamOccurence == 1: 154 self.rollerCondition = -1 155 if abs(self.rollerMotorEncoder.getVelocity()) >= self.unjam: 156 self.jamOccurence = 0 157 else: 158 if self.rollerMotorEncoder.getVelocity() <= self.unjam: 159 wpilib.Alert("Jam reversal unsuccessful! Stopping motor.", wpilib.Alert.AlertType.kError) 160 self.rollerMotor.disable() 161 # if self.rollerSensor == 0: 162 # self.deactivateRoller() 163 # else: 164 self.rollerCondition = 1 165 self.jamOccurence = 0 166 self.jamDetected = False 167 168 # def updateIntake(self, newIntakeVelocity): 169 # self.intakeVelocity = newIntakeVelocity 170 171 def updateRoller(self, newRollerVelocity): 172 self.rollerVelocity = newRollerVelocity 173 174 # def automaticRollerActivation(self): 175 # if not self.breakBeam.get(): 176 # self.rollerSensor = 1 177 # self.activateRoller() 178 # else: 179 # if self.rollerSensor == 1: 180 # self.deactivateRoller() 181 # self.rollerSensor = 0 182 183 # def intakeSlowdown(self): 184 # self.intakeDifference = abs(self.intakeStowed) + abs(self.intakeDeployed) 185 # if self.intakeCondition == 1: 186 # self.remainingRotations = self.intakeDifference - (abs(self.intakeStowed) + abs(0 - self.intakeMotorEncoder.getPosition())) 187 # self.intakeSlowdownPosition = self.intakeStowed + (self.intakeDifference * 0.75) 188 # if self.intakeMotorEncoder.getPosition() >= self.intakeSlowdownPosition: 189 # self.intakeCondition = 0.5 190 # if self.intakeCondition == -1: 191 # self.remainingRotations = self.intakeDifference - (self.intakeDeployed - self.intakeMotorEncoder.getPosition() - abs(self.intakeStowed)) 192 # self.intakeSlowdownPosition = self.intakeDeployed - (self.intakeDifference * 0.75) 193 # if self.intakeMotorEncoder.getPosition() <= self.intakeSlowdownPosition: 194 # self.intakeCondition = -0.5 195 196 # def rampIntake(self): 197 # if not self.intakeRampedCondition: 198 # self.intakeDifference = abs(self.intakeStowed) + abs(self.intakeDeployed) 199 # self.intakeRamp = self.intakeStowed + (self.intakeDifference * 0.5) 200 # if self.intakeMotorEncoder.getPosition() <= self.intakeRamp: 201 # if self.intakeRamped <= 0: 202 # self.baselineFault = time.perf_counter() 203 # self.intakeRamped = 1 204 # self.intakeCondition = 1 205 # self.intakeRampStatus = 1 206 # elif self.intakeMotorEncoder.getPosition() >= self.intakeRamp: 207 # if self.intakeRamped >= 0: 208 # self.baselineFault = time.perf_counter() 209 # self.intakeRamped = -1 210 # self.intakeCondition = -1 211 # self.intakeRampStatus = 1 212 213 def motorChecks(self): 214 #Check if intake deployment motor is deploying without limits 215 # if self.intakeMotorEncoder.getPosition() >= self.intakeDeployed + 15 and self.intakeCondition >= 0: 216 # wpilib.Alert("INTAKE ERR122: Intake Motor appears to be deploying outside of limits! Motor has been disabled.", wpilib.Alert.AlertType.kError) 217 # self.intakeMotor.disable() 218 219 # if self.intakeMotorEncoder.getPosition() <= self.intakeStowed - 15 and self.intakeCondition <= 0: 220 # wpilib.Alert("INTAKE ERR122: Intake Motor appears to be stowing outside of limits! Motor has been disabled.", wpilib.Alert.AlertType.kError) 221 # self.intakeMotor.disable() 222 223 224 #Stop intake deployment motor if it reaches limits 225 # if self.intakeMotorEncoder.getPosition() >= self.intakeDeployed and self.intakeCondition >= 0: 226 # self.intakeCondition = 0 227 # if self.intakeMotorEncoder.getPosition() <= self.intakeStowed and self.intakeCondition <= 0: 228 # self.intakeCondition = 0 229 230 231 #Stop intake deployment motor if it's position does not change even when it is supposed to be moving 232 # self.intakeMotorPositions.pop(0) 233 # self.intakeMotorPositions.append(self.intakeMotorEncoder.getPosition()) 234 # if not self.intakeMotorEncoder.getPosition() <= self.intakeStowed and not self.intakeMotorEncoder.getPosition() >= self.intakeDeployed: 235 # if self.intakeMotorPositions.count(self.intakeMotorEncoder.getPosition()) == 5: 236 # if self.intakeCondition == -1: 237 # self.intakeStowed = self.intakeMotorEncoder.getPosition() + 1 238 # self.intakeCondition = 0 239 # elif self.intakeCondition == 1: 240 # self.intakeDeployed = self.intakeMotorEncoder.getPosition() - 1 241 # self.intakeCondition = 0 242 243 # if self.intakeCondition == 0: 244 # self.intakeVelocity = 0 245 self.rollerMotor.set(self.rollerCondition * self.rollerVelocity) 246 247 # self.intakeMotorPID.setReference( 248 # self.intakeCondition * self.intakeVelocity, rev.SparkLowLevel.ControlType.kVelocity, rev.ClosedLoopSlot.kSlot0 249 # ) 250 251 #Stop intake deployment motor if it is being ramped 252 # if self.intakeRampStatus == 1: 253 # if self.intakeRamped == 1: 254 # if self.intakeMotorEncoder.getPosition() >= self.intakeRamp: 255 # self.intakeRamped = 0 256 # self.intakeCondition = 0 257 # self.intakeRampedCondition = True 258 # if self.intakeRamped == -1: 259 # if self.intakeMotorEncoder.getPosition() <= self.intakeRamp: 260 # self.intakeRamped = 0 261 # self.intakeCondition = 0 262 # self.intakeRampedCondition = True 263 264 #Allows intake to be ramped even from deployed/stowed position 265 # if self.intakeMotorEncoder.getPosition() >= self.intakeDeployed: 266 # self.intakeRamped = 0 267 # if self.intakeMotorEncoder.getPosition() <= self.intakeStowed: 268 # self.intakeRamped = 0 269 270 # if self.intakeCondition != 0: 271 # self.intakeRampedCondition = False 272 273 # def tuningMotors(self): 274 # configUse = ( 275 # self.intakeMotorConfig.closedLoop 276 # .setFeedbackSensor(rev.FeedbackSensor.kPrimaryEncoder) 277 # .pidf(*OperatorRobotConfig.intake_pid) 278 # ) 279 280 # self.intakeMotor.configure( 281 # configUse, rev.ResetMode.kNoResetSafeParemeters, 282 # rev.PersistMode.kPersistParameters 283 # ) 284 285 # self.intakeMotorPID = self.intakeMotor.getClosedLoopController() 286 # self.state_speed = 0 287 288 289 def periodic(self): 290 # wpilib.SmartDashboard.putNumber("Intake Position", self.intakeMotorEncoder.getPosition()) 291 wpilib.SmartDashboard.putNumber("Roller Position", self.rollerMotorEncoder.getPosition()) 292 # wpilib.SmartDashboard.putNumber("Intake Deployed", self.intakeDeployed) 293 # wpilib.SmartDashboard.putBoolean("Hall Effects Sensor", self.HallEffectSensor.get()) 294 wpilib.SmartDashboard.putNumber("Time", time.perf_counter()) 295 wpilib.SmartDashboard.putNumber("Baseline Fault", self.baselineFault) 296 # wpilib.SmartDashboard.putNumber("Intake Condition", self.intakeCondition) 297 # wpilib.SmartDashboard.putBoolean("Break Beam Sensor", self.breakBeam.get()) 298 wpilib.SmartDashboard.putNumber("Roller Sensor", self.rollerSensor) 299 # wpilib.SmartDashboard.putNumber("Intake Difference", self.intakeDifference) 300 # wpilib.SmartDashboard.putNumber("Remaining Rotations", self.remainingRotations) 301 # wpilib.SmartDashboard.putNumber("Intake Slowdown Position", self.intakeSlowdownPosition) 302 # wpilib.SmartDashboard.putNumber("Intake Ramped", self.intakeRamped) 303 # wpilib.SmartDashboard.putNumber("Intake Ramp Position", self.intakeRamp) 304 # wpilib.SmartDashboard.putBoolean("Intake Ramp Condition", self.intakeRampedCondition) 305 # wpilib.SmartDashboard.putNumberArray("Intake Positions", self.intakeMotorPositions) 306 # wpilib.SmartDashboard.putNumber("Intake Stowed", self.intakeStowed) 307 wpilib.SmartDashboard.putNumber("Roller Condition", self.rollerCondition) 308 wpilib.SmartDashboard.putBoolean("Roller Jam", self.jamDetected) 309 wpilib.SmartDashboard.putNumber("Actual Roller Velocity", self.rollerMotorEncoder.getVelocity()) 310 wpilib.SmartDashboard.putNumber("Baseline Detected Jam", self.baselineDetectedJam) 311 312 self.motorChecks() 313 # self.automaticRollerActivation() 314 # self.intakeSlowdown() 315 self.jamDetection()
intakeVelocity =
0.3
intakeMotorThreshold =
1
intakeFaultThreshold =
10
rollerVelocity =
0.3
rollerFaultThreshold =
10
jamFaultThreshold =
10
baselineFault =
0
baselineJam =
0
jamReversalCount =
0
class
IntakeSubsystem(commands2.subsystem.Subsystem):
22class IntakeSubsystem(commands2.SubsystemBase): 23 def __init__(self, alternateConfiguration = False): 24 #Initializes all devices 25 #Alternate Configuration effectively just switches the motors used for intake deployment and rollers 26 if alternateConfiguration == True: 27 # self.intakeMotor = rev.SparkMax(intakeConsts.kRollerMotorCanId, rev.SparkLowLevel.MotorType.kBrushless) 28 # self.intakeMotorEncoder = self.intakeMotor.getEncoder() 29 # self.intakeMotorConfig = rev.SparkMaxConfig() 30 # self.tuningMotors() 31 #self.intakeMotorPosition = self.intakeMotorEncoder.getPosition() 32 33 self.rollerMotor = rev.SparkFlex(intakeConsts.kIntakeMotorCanId, rev.SparkLowLevel.MotorType.kBrushless) 34 self.rollerMotorEncoder = self.rollerMotor.getEncoder() 35 self.rollerMotorVelocity = self.rollerMotorEncoder.getVelocity() 36 else: 37 # self.intakeMotor = rev.SparkFlex(intakeConsts.kIntakeMotorCanId, rev.SparkLowLevel.MotorType.kBrushless) 38 # self.intakeMotorEncoder = self.intakeMotor.getEncoder() 39 #self.intakeMotorPosition = self.intakeMotorEncoder.getPosition() 40 41 self.rollerMotor = rev.SparkMax(intakeConsts.kRollerMotorCanId, rev.SparkLowLevel.MotorType.kBrushless) 42 self.rollerMotorEncoder = self.rollerMotor.getEncoder() 43 self.rollerMotorVelocity = self.rollerMotorEncoder.getVelocity() 44 45 # self.breakBeam = wpilib.DigitalInput(intakeConsts.kBreakBeam) 46 # self.frontBreakbeam = wpilib.DigitalInput(intakeConsts.kFrontBreakBeam) 47 # self.backBreakbeam = wpilib.DigitalInput(intakeConsts.kBackBreakBeam) 48 # self.frontBeamBroken = not self.frontBreakbeam.get() 49 # self.backBeamBroken = not self.backBreakbeam.get() 50 51 # self.HallEffectSensor = wpilib.DigitalInput(intakeConsts.kHallEffectSensor) 52 53 #Set Variables 54 # self.intakeDeployed = 200 #Minimum amount of rotations before assuming intake is deployed 55 # self.intakeStowed = 0 #Maximum amount of rotations before assuming intake is stowed 56 # self.intakeFaultThreshold = 2 #Amount of time spent trying to deploy/stow intake before fault condition is triggered 57 # self.intakeMagnetFaultThreshold = 2 #Amount of time before magnets need to have stopped tripping hall effects sensor or fault condition is triggered 58 self.rollerFaultThreshold = 2 #Amount of time spent trying to operate rollers before fault condition is triggered 59 self.jamFaultThreshold = 0 #Amount of attempts done trying to reverse rollers in the event of a jam before a fault condition is triggered 60 self.jamTime = 1 #Amount of time to wait before assuming a ball inside the intake has gotten stuck 61 self.jamThreshold = 10 #Maximum sustained rpm before assuming a ball inside the rollers has gotten stuck 62 self.jamReversalTime = 3 #Amount of time to have motors reverse when a ball inside the intake has gotten stuck 63 self.unjam = 1500 #Minimum sustained rpm before assuming rollers have been unjammed 64 65 # self.intakeCondition = 0 #Leave at 0, provides reference to code on current intake status 66 # self.intakeRamped = 0 #Leave at 0, provides reference to code on ramping intake status 67 # self.intakeRampedCondition = 0 #Leave at 0, provides reference to code on whether ramping intake is finished 68 # self.intakeVelocity = 0 #Leave at 0, any updating is to be done thru Network Table, Speed (in rpm) in which the intake motor will move upon deployment/stowing 69 self.rollerVelocity = 0 #Leave at 0, any updating is to be done thru Network Table, Speed in which the roller motor will move upon deployment 70 self.baselineFault = 0 #Leave at 0, provides baseline to compare to when determining faults 71 self.baselineJam = 0 #Leave at 0, provides baseline to compare to when determining faults 72 self.jamReversalCount = 0 #Leave at 0, stores amount of attempts in reversing motors in the event of a jam before a fault condition is triggered 73 # self.intakeDifference = 0 #Leave at 0, rotations required to get from intake stowed position to intake deployed position is automatically calculated 74 # self.remainingRotations = 0 #Leave at 0, rotations remaining to finish deploying/stowing intake is automatically calculated 75 # self.intakeSlowdownPosition = 0 #Leave at 0, stores amount of intake motor rotations required to slow it down 76 # self.intakeRamp = 0 #Leave at 0, motor position for ramp is automatically calculated 77 # self.intakeRampStatus = 0 #Leave at 0, provides reference to code on whether intake is moving to ramp 78 # self.hardStopIndex = 0 #Leave at 0, provides index to code for hardstop checks 79 self.jamOccurence = 0 #Leave at 0, provides baseline to compare to when determining jams 80 self.baselineDetectedJam = 0 #Leave at 0, provides baseline to compare to when jam detection is activated 81 self.rollerCondition = 0 #Leave at 0, provides reference to code on current roller status 82 self.rollerSensor = 0 #Leave at 0, ensures that the rollers are stopped only once, preventing obstruction of manual controls 83 84 self.jamDetected = False #Leave at False 85 # self.intakeMotorPositions = arr.array('f', [0,0,0,0,0]) #Leave with all zeros, for checking if intake motor stopped during deployment/stowing 86 87 # def deployIntake(self): 88 # #Check Sensor for deployment, if not, deploy it. 89 # if self.intakeCondition <= 0 and self.intakeMotorEncoder.getPosition() <= self.intakeDeployed: 90 # self.baselineFault = time.perf_counter() 91 # self.intakeCondition = 1 92 # if self.intakeCondition >= 0: 93 # if self.HallEffectSensor.get() == False: 94 # self.intakeDeployed = self.intakeMotorEncoder.getPosition() 95 # self.intakeCondition = 0 96 # if self.intakeMotorEncoder.getPosition() >= self.intakeDeployed: 97 # self.intakeCondition = 0 98 # if self.baselineFault - time.perf_counter() >= self.intakeFaultThreshold: 99 # wpilib.Alert("INTAKE ERR101: Deployment of intake dosen't appear to be working! Stopped activation.", wpilib.Alert.AlertType.kError) 100 # return 101 # else: 102 # self.intakeCondition = 0 103 104 def activateRoller(self): 105 # self.baselineFault = time.perf_counter() 106 107 # Apply voltage to roller until it starts moving; Terminate program with ERR103 if fault condition is detected 108 # while self.rollerMotorEncoder.getVelocity() == 0: 109 if self.rollerCondition != 1: 110 self.rollerCondition = 1 111 # if baselineFault - time.perf_counter() >= self.rollerFaultThreshold: 112 # wpilib.Alert("INTAKE ERR103: Activation of rollers don't appear to be working! Stopped activation.", wpilib.Alert.AlertType.kError) 113 # return 114 115 def deactivateRoller(self): 116 # self.baselineFault = time.perf_counter() 117 118 # Try to terminate voltage until motor stops moving; Terminate program with ERR103 if fault condition is detected 119 # while self.rollerMotorEncoder.getVelocity() != 0: 120 if self.rollerCondition != 0: 121 self.rollerCondition = 0 122 # if baselineFault - time.perf_counter() >= self.rollerFaultThreshold: 123 # wpilib.Alert("INTAKE ERR103: Activation of rollers don't appear to be working! Stopped activation.", wpilib.Alert.AlertType.kError) 124 # return 125 126 # def stowIntake(self): 127 # if self.intakeCondition >= 0 and self.intakeMotorEncoder.getPosition() >= self.intakeStowed: 128 # self.baselineFault = time.perf_counter() 129 # self.intakeCondition = -1 130 # if self.intakeCondition <= 0: 131 # if self.intakeMotorEncoder.getPosition() <= self.intakeStowed: 132 # self.intakeCondition = 0 133 # if self.baselineFault - time.perf_counter() >= self.intakeFaultThreshold: 134 # wpilib.Alert("INTAKE ERR112: Intake Stow doesn't appear to be working! Stopping activation.", wpilib.Alert.AlertType.kError) 135 # return 136 # if self.intakeMagnetFaultThreshold + 1 >= time.perf_counter() - self.baselineFault >= self.intakeMagnetFaultThreshold: 137 # if self.HallEffectSensor.get() == False: 138 # wpilib.Alert("INTAKE ERR112: Intake motor is engaged but the Intake doesn't appear to be moving! Stopping code.", wpilib.Alert.AlertType.kError) 139 # return 140 # else: 141 # self.intakeCondition = 0 142 143 def jamDetection(self): 144 if not self.jamDetected: 145 if self.rollerMotorEncoder.getVelocity() <= self.jamThreshold and self.rollerCondition == 1: 146 if self.jamOccurence == 0: 147 self.baselineJam = time.perf_counter() 148 self.jamOccurence = 1 149 else: 150 if time.perf_counter() - self.baselineJam >= self.jamTime: 151 self.baselineDetectedJam = time.perf_counter() 152 self.jamDetected = True 153 else: 154 if time.perf_counter() - self.baselineDetectedJam <= self.jamReversalTime and self.jamOccurence == 1: 155 self.rollerCondition = -1 156 if abs(self.rollerMotorEncoder.getVelocity()) >= self.unjam: 157 self.jamOccurence = 0 158 else: 159 if self.rollerMotorEncoder.getVelocity() <= self.unjam: 160 wpilib.Alert("Jam reversal unsuccessful! Stopping motor.", wpilib.Alert.AlertType.kError) 161 self.rollerMotor.disable() 162 # if self.rollerSensor == 0: 163 # self.deactivateRoller() 164 # else: 165 self.rollerCondition = 1 166 self.jamOccurence = 0 167 self.jamDetected = False 168 169 # def updateIntake(self, newIntakeVelocity): 170 # self.intakeVelocity = newIntakeVelocity 171 172 def updateRoller(self, newRollerVelocity): 173 self.rollerVelocity = newRollerVelocity 174 175 # def automaticRollerActivation(self): 176 # if not self.breakBeam.get(): 177 # self.rollerSensor = 1 178 # self.activateRoller() 179 # else: 180 # if self.rollerSensor == 1: 181 # self.deactivateRoller() 182 # self.rollerSensor = 0 183 184 # def intakeSlowdown(self): 185 # self.intakeDifference = abs(self.intakeStowed) + abs(self.intakeDeployed) 186 # if self.intakeCondition == 1: 187 # self.remainingRotations = self.intakeDifference - (abs(self.intakeStowed) + abs(0 - self.intakeMotorEncoder.getPosition())) 188 # self.intakeSlowdownPosition = self.intakeStowed + (self.intakeDifference * 0.75) 189 # if self.intakeMotorEncoder.getPosition() >= self.intakeSlowdownPosition: 190 # self.intakeCondition = 0.5 191 # if self.intakeCondition == -1: 192 # self.remainingRotations = self.intakeDifference - (self.intakeDeployed - self.intakeMotorEncoder.getPosition() - abs(self.intakeStowed)) 193 # self.intakeSlowdownPosition = self.intakeDeployed - (self.intakeDifference * 0.75) 194 # if self.intakeMotorEncoder.getPosition() <= self.intakeSlowdownPosition: 195 # self.intakeCondition = -0.5 196 197 # def rampIntake(self): 198 # if not self.intakeRampedCondition: 199 # self.intakeDifference = abs(self.intakeStowed) + abs(self.intakeDeployed) 200 # self.intakeRamp = self.intakeStowed + (self.intakeDifference * 0.5) 201 # if self.intakeMotorEncoder.getPosition() <= self.intakeRamp: 202 # if self.intakeRamped <= 0: 203 # self.baselineFault = time.perf_counter() 204 # self.intakeRamped = 1 205 # self.intakeCondition = 1 206 # self.intakeRampStatus = 1 207 # elif self.intakeMotorEncoder.getPosition() >= self.intakeRamp: 208 # if self.intakeRamped >= 0: 209 # self.baselineFault = time.perf_counter() 210 # self.intakeRamped = -1 211 # self.intakeCondition = -1 212 # self.intakeRampStatus = 1 213 214 def motorChecks(self): 215 #Check if intake deployment motor is deploying without limits 216 # if self.intakeMotorEncoder.getPosition() >= self.intakeDeployed + 15 and self.intakeCondition >= 0: 217 # wpilib.Alert("INTAKE ERR122: Intake Motor appears to be deploying outside of limits! Motor has been disabled.", wpilib.Alert.AlertType.kError) 218 # self.intakeMotor.disable() 219 220 # if self.intakeMotorEncoder.getPosition() <= self.intakeStowed - 15 and self.intakeCondition <= 0: 221 # wpilib.Alert("INTAKE ERR122: Intake Motor appears to be stowing outside of limits! Motor has been disabled.", wpilib.Alert.AlertType.kError) 222 # self.intakeMotor.disable() 223 224 225 #Stop intake deployment motor if it reaches limits 226 # if self.intakeMotorEncoder.getPosition() >= self.intakeDeployed and self.intakeCondition >= 0: 227 # self.intakeCondition = 0 228 # if self.intakeMotorEncoder.getPosition() <= self.intakeStowed and self.intakeCondition <= 0: 229 # self.intakeCondition = 0 230 231 232 #Stop intake deployment motor if it's position does not change even when it is supposed to be moving 233 # self.intakeMotorPositions.pop(0) 234 # self.intakeMotorPositions.append(self.intakeMotorEncoder.getPosition()) 235 # if not self.intakeMotorEncoder.getPosition() <= self.intakeStowed and not self.intakeMotorEncoder.getPosition() >= self.intakeDeployed: 236 # if self.intakeMotorPositions.count(self.intakeMotorEncoder.getPosition()) == 5: 237 # if self.intakeCondition == -1: 238 # self.intakeStowed = self.intakeMotorEncoder.getPosition() + 1 239 # self.intakeCondition = 0 240 # elif self.intakeCondition == 1: 241 # self.intakeDeployed = self.intakeMotorEncoder.getPosition() - 1 242 # self.intakeCondition = 0 243 244 # if self.intakeCondition == 0: 245 # self.intakeVelocity = 0 246 self.rollerMotor.set(self.rollerCondition * self.rollerVelocity) 247 248 # self.intakeMotorPID.setReference( 249 # self.intakeCondition * self.intakeVelocity, rev.SparkLowLevel.ControlType.kVelocity, rev.ClosedLoopSlot.kSlot0 250 # ) 251 252 #Stop intake deployment motor if it is being ramped 253 # if self.intakeRampStatus == 1: 254 # if self.intakeRamped == 1: 255 # if self.intakeMotorEncoder.getPosition() >= self.intakeRamp: 256 # self.intakeRamped = 0 257 # self.intakeCondition = 0 258 # self.intakeRampedCondition = True 259 # if self.intakeRamped == -1: 260 # if self.intakeMotorEncoder.getPosition() <= self.intakeRamp: 261 # self.intakeRamped = 0 262 # self.intakeCondition = 0 263 # self.intakeRampedCondition = True 264 265 #Allows intake to be ramped even from deployed/stowed position 266 # if self.intakeMotorEncoder.getPosition() >= self.intakeDeployed: 267 # self.intakeRamped = 0 268 # if self.intakeMotorEncoder.getPosition() <= self.intakeStowed: 269 # self.intakeRamped = 0 270 271 # if self.intakeCondition != 0: 272 # self.intakeRampedCondition = False 273 274 # def tuningMotors(self): 275 # configUse = ( 276 # self.intakeMotorConfig.closedLoop 277 # .setFeedbackSensor(rev.FeedbackSensor.kPrimaryEncoder) 278 # .pidf(*OperatorRobotConfig.intake_pid) 279 # ) 280 281 # self.intakeMotor.configure( 282 # configUse, rev.ResetMode.kNoResetSafeParemeters, 283 # rev.PersistMode.kPersistParameters 284 # ) 285 286 # self.intakeMotorPID = self.intakeMotor.getClosedLoopController() 287 # self.state_speed = 0 288 289 290 def periodic(self): 291 # wpilib.SmartDashboard.putNumber("Intake Position", self.intakeMotorEncoder.getPosition()) 292 wpilib.SmartDashboard.putNumber("Roller Position", self.rollerMotorEncoder.getPosition()) 293 # wpilib.SmartDashboard.putNumber("Intake Deployed", self.intakeDeployed) 294 # wpilib.SmartDashboard.putBoolean("Hall Effects Sensor", self.HallEffectSensor.get()) 295 wpilib.SmartDashboard.putNumber("Time", time.perf_counter()) 296 wpilib.SmartDashboard.putNumber("Baseline Fault", self.baselineFault) 297 # wpilib.SmartDashboard.putNumber("Intake Condition", self.intakeCondition) 298 # wpilib.SmartDashboard.putBoolean("Break Beam Sensor", self.breakBeam.get()) 299 wpilib.SmartDashboard.putNumber("Roller Sensor", self.rollerSensor) 300 # wpilib.SmartDashboard.putNumber("Intake Difference", self.intakeDifference) 301 # wpilib.SmartDashboard.putNumber("Remaining Rotations", self.remainingRotations) 302 # wpilib.SmartDashboard.putNumber("Intake Slowdown Position", self.intakeSlowdownPosition) 303 # wpilib.SmartDashboard.putNumber("Intake Ramped", self.intakeRamped) 304 # wpilib.SmartDashboard.putNumber("Intake Ramp Position", self.intakeRamp) 305 # wpilib.SmartDashboard.putBoolean("Intake Ramp Condition", self.intakeRampedCondition) 306 # wpilib.SmartDashboard.putNumberArray("Intake Positions", self.intakeMotorPositions) 307 # wpilib.SmartDashboard.putNumber("Intake Stowed", self.intakeStowed) 308 wpilib.SmartDashboard.putNumber("Roller Condition", self.rollerCondition) 309 wpilib.SmartDashboard.putBoolean("Roller Jam", self.jamDetected) 310 wpilib.SmartDashboard.putNumber("Actual Roller Velocity", self.rollerMotorEncoder.getVelocity()) 311 wpilib.SmartDashboard.putNumber("Baseline Detected Jam", self.baselineDetectedJam) 312 313 self.motorChecks() 314 # self.automaticRollerActivation() 315 # self.intakeSlowdown() 316 self.jamDetection()
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.
IntakeSubsystem(alternateConfiguration=False)
23 def __init__(self, alternateConfiguration = False): 24 #Initializes all devices 25 #Alternate Configuration effectively just switches the motors used for intake deployment and rollers 26 if alternateConfiguration == True: 27 # self.intakeMotor = rev.SparkMax(intakeConsts.kRollerMotorCanId, rev.SparkLowLevel.MotorType.kBrushless) 28 # self.intakeMotorEncoder = self.intakeMotor.getEncoder() 29 # self.intakeMotorConfig = rev.SparkMaxConfig() 30 # self.tuningMotors() 31 #self.intakeMotorPosition = self.intakeMotorEncoder.getPosition() 32 33 self.rollerMotor = rev.SparkFlex(intakeConsts.kIntakeMotorCanId, rev.SparkLowLevel.MotorType.kBrushless) 34 self.rollerMotorEncoder = self.rollerMotor.getEncoder() 35 self.rollerMotorVelocity = self.rollerMotorEncoder.getVelocity() 36 else: 37 # self.intakeMotor = rev.SparkFlex(intakeConsts.kIntakeMotorCanId, rev.SparkLowLevel.MotorType.kBrushless) 38 # self.intakeMotorEncoder = self.intakeMotor.getEncoder() 39 #self.intakeMotorPosition = self.intakeMotorEncoder.getPosition() 40 41 self.rollerMotor = rev.SparkMax(intakeConsts.kRollerMotorCanId, rev.SparkLowLevel.MotorType.kBrushless) 42 self.rollerMotorEncoder = self.rollerMotor.getEncoder() 43 self.rollerMotorVelocity = self.rollerMotorEncoder.getVelocity() 44 45 # self.breakBeam = wpilib.DigitalInput(intakeConsts.kBreakBeam) 46 # self.frontBreakbeam = wpilib.DigitalInput(intakeConsts.kFrontBreakBeam) 47 # self.backBreakbeam = wpilib.DigitalInput(intakeConsts.kBackBreakBeam) 48 # self.frontBeamBroken = not self.frontBreakbeam.get() 49 # self.backBeamBroken = not self.backBreakbeam.get() 50 51 # self.HallEffectSensor = wpilib.DigitalInput(intakeConsts.kHallEffectSensor) 52 53 #Set Variables 54 # self.intakeDeployed = 200 #Minimum amount of rotations before assuming intake is deployed 55 # self.intakeStowed = 0 #Maximum amount of rotations before assuming intake is stowed 56 # self.intakeFaultThreshold = 2 #Amount of time spent trying to deploy/stow intake before fault condition is triggered 57 # self.intakeMagnetFaultThreshold = 2 #Amount of time before magnets need to have stopped tripping hall effects sensor or fault condition is triggered 58 self.rollerFaultThreshold = 2 #Amount of time spent trying to operate rollers before fault condition is triggered 59 self.jamFaultThreshold = 0 #Amount of attempts done trying to reverse rollers in the event of a jam before a fault condition is triggered 60 self.jamTime = 1 #Amount of time to wait before assuming a ball inside the intake has gotten stuck 61 self.jamThreshold = 10 #Maximum sustained rpm before assuming a ball inside the rollers has gotten stuck 62 self.jamReversalTime = 3 #Amount of time to have motors reverse when a ball inside the intake has gotten stuck 63 self.unjam = 1500 #Minimum sustained rpm before assuming rollers have been unjammed 64 65 # self.intakeCondition = 0 #Leave at 0, provides reference to code on current intake status 66 # self.intakeRamped = 0 #Leave at 0, provides reference to code on ramping intake status 67 # self.intakeRampedCondition = 0 #Leave at 0, provides reference to code on whether ramping intake is finished 68 # self.intakeVelocity = 0 #Leave at 0, any updating is to be done thru Network Table, Speed (in rpm) in which the intake motor will move upon deployment/stowing 69 self.rollerVelocity = 0 #Leave at 0, any updating is to be done thru Network Table, Speed in which the roller motor will move upon deployment 70 self.baselineFault = 0 #Leave at 0, provides baseline to compare to when determining faults 71 self.baselineJam = 0 #Leave at 0, provides baseline to compare to when determining faults 72 self.jamReversalCount = 0 #Leave at 0, stores amount of attempts in reversing motors in the event of a jam before a fault condition is triggered 73 # self.intakeDifference = 0 #Leave at 0, rotations required to get from intake stowed position to intake deployed position is automatically calculated 74 # self.remainingRotations = 0 #Leave at 0, rotations remaining to finish deploying/stowing intake is automatically calculated 75 # self.intakeSlowdownPosition = 0 #Leave at 0, stores amount of intake motor rotations required to slow it down 76 # self.intakeRamp = 0 #Leave at 0, motor position for ramp is automatically calculated 77 # self.intakeRampStatus = 0 #Leave at 0, provides reference to code on whether intake is moving to ramp 78 # self.hardStopIndex = 0 #Leave at 0, provides index to code for hardstop checks 79 self.jamOccurence = 0 #Leave at 0, provides baseline to compare to when determining jams 80 self.baselineDetectedJam = 0 #Leave at 0, provides baseline to compare to when jam detection is activated 81 self.rollerCondition = 0 #Leave at 0, provides reference to code on current roller status 82 self.rollerSensor = 0 #Leave at 0, ensures that the rollers are stopped only once, preventing obstruction of manual controls 83 84 self.jamDetected = False #Leave at False 85 # self.intakeMotorPositions = arr.array('f', [0,0,0,0,0]) #Leave with all zeros, for checking if intake motor stopped during deployment/stowing
__init__(self: wpiutil._wpiutil.Sendable) -> None
def
activateRoller(self):
104 def activateRoller(self): 105 # self.baselineFault = time.perf_counter() 106 107 # Apply voltage to roller until it starts moving; Terminate program with ERR103 if fault condition is detected 108 # while self.rollerMotorEncoder.getVelocity() == 0: 109 if self.rollerCondition != 1: 110 self.rollerCondition = 1 111 # if baselineFault - time.perf_counter() >= self.rollerFaultThreshold: 112 # wpilib.Alert("INTAKE ERR103: Activation of rollers don't appear to be working! Stopped activation.", wpilib.Alert.AlertType.kError) 113 # return
def
deactivateRoller(self):
115 def deactivateRoller(self): 116 # self.baselineFault = time.perf_counter() 117 118 # Try to terminate voltage until motor stops moving; Terminate program with ERR103 if fault condition is detected 119 # while self.rollerMotorEncoder.getVelocity() != 0: 120 if self.rollerCondition != 0: 121 self.rollerCondition = 0 122 # if baselineFault - time.perf_counter() >= self.rollerFaultThreshold: 123 # wpilib.Alert("INTAKE ERR103: Activation of rollers don't appear to be working! Stopped activation.", wpilib.Alert.AlertType.kError) 124 # return
def
jamDetection(self):
143 def jamDetection(self): 144 if not self.jamDetected: 145 if self.rollerMotorEncoder.getVelocity() <= self.jamThreshold and self.rollerCondition == 1: 146 if self.jamOccurence == 0: 147 self.baselineJam = time.perf_counter() 148 self.jamOccurence = 1 149 else: 150 if time.perf_counter() - self.baselineJam >= self.jamTime: 151 self.baselineDetectedJam = time.perf_counter() 152 self.jamDetected = True 153 else: 154 if time.perf_counter() - self.baselineDetectedJam <= self.jamReversalTime and self.jamOccurence == 1: 155 self.rollerCondition = -1 156 if abs(self.rollerMotorEncoder.getVelocity()) >= self.unjam: 157 self.jamOccurence = 0 158 else: 159 if self.rollerMotorEncoder.getVelocity() <= self.unjam: 160 wpilib.Alert("Jam reversal unsuccessful! Stopping motor.", wpilib.Alert.AlertType.kError) 161 self.rollerMotor.disable() 162 # if self.rollerSensor == 0: 163 # self.deactivateRoller() 164 # else: 165 self.rollerCondition = 1 166 self.jamOccurence = 0 167 self.jamDetected = False
def
motorChecks(self):
214 def motorChecks(self): 215 #Check if intake deployment motor is deploying without limits 216 # if self.intakeMotorEncoder.getPosition() >= self.intakeDeployed + 15 and self.intakeCondition >= 0: 217 # wpilib.Alert("INTAKE ERR122: Intake Motor appears to be deploying outside of limits! Motor has been disabled.", wpilib.Alert.AlertType.kError) 218 # self.intakeMotor.disable() 219 220 # if self.intakeMotorEncoder.getPosition() <= self.intakeStowed - 15 and self.intakeCondition <= 0: 221 # wpilib.Alert("INTAKE ERR122: Intake Motor appears to be stowing outside of limits! Motor has been disabled.", wpilib.Alert.AlertType.kError) 222 # self.intakeMotor.disable() 223 224 225 #Stop intake deployment motor if it reaches limits 226 # if self.intakeMotorEncoder.getPosition() >= self.intakeDeployed and self.intakeCondition >= 0: 227 # self.intakeCondition = 0 228 # if self.intakeMotorEncoder.getPosition() <= self.intakeStowed and self.intakeCondition <= 0: 229 # self.intakeCondition = 0 230 231 232 #Stop intake deployment motor if it's position does not change even when it is supposed to be moving 233 # self.intakeMotorPositions.pop(0) 234 # self.intakeMotorPositions.append(self.intakeMotorEncoder.getPosition()) 235 # if not self.intakeMotorEncoder.getPosition() <= self.intakeStowed and not self.intakeMotorEncoder.getPosition() >= self.intakeDeployed: 236 # if self.intakeMotorPositions.count(self.intakeMotorEncoder.getPosition()) == 5: 237 # if self.intakeCondition == -1: 238 # self.intakeStowed = self.intakeMotorEncoder.getPosition() + 1 239 # self.intakeCondition = 0 240 # elif self.intakeCondition == 1: 241 # self.intakeDeployed = self.intakeMotorEncoder.getPosition() - 1 242 # self.intakeCondition = 0 243 244 # if self.intakeCondition == 0: 245 # self.intakeVelocity = 0 246 self.rollerMotor.set(self.rollerCondition * self.rollerVelocity) 247 248 # self.intakeMotorPID.setReference( 249 # self.intakeCondition * self.intakeVelocity, rev.SparkLowLevel.ControlType.kVelocity, rev.ClosedLoopSlot.kSlot0 250 # ) 251 252 #Stop intake deployment motor if it is being ramped 253 # if self.intakeRampStatus == 1: 254 # if self.intakeRamped == 1: 255 # if self.intakeMotorEncoder.getPosition() >= self.intakeRamp: 256 # self.intakeRamped = 0 257 # self.intakeCondition = 0 258 # self.intakeRampedCondition = True 259 # if self.intakeRamped == -1: 260 # if self.intakeMotorEncoder.getPosition() <= self.intakeRamp: 261 # self.intakeRamped = 0 262 # self.intakeCondition = 0 263 # self.intakeRampedCondition = True 264 265 #Allows intake to be ramped even from deployed/stowed position 266 # if self.intakeMotorEncoder.getPosition() >= self.intakeDeployed: 267 # self.intakeRamped = 0 268 # if self.intakeMotorEncoder.getPosition() <= self.intakeStowed: 269 # self.intakeRamped = 0 270 271 # if self.intakeCondition != 0: 272 # self.intakeRampedCondition = False
def
periodic(self):
290 def periodic(self): 291 # wpilib.SmartDashboard.putNumber("Intake Position", self.intakeMotorEncoder.getPosition()) 292 wpilib.SmartDashboard.putNumber("Roller Position", self.rollerMotorEncoder.getPosition()) 293 # wpilib.SmartDashboard.putNumber("Intake Deployed", self.intakeDeployed) 294 # wpilib.SmartDashboard.putBoolean("Hall Effects Sensor", self.HallEffectSensor.get()) 295 wpilib.SmartDashboard.putNumber("Time", time.perf_counter()) 296 wpilib.SmartDashboard.putNumber("Baseline Fault", self.baselineFault) 297 # wpilib.SmartDashboard.putNumber("Intake Condition", self.intakeCondition) 298 # wpilib.SmartDashboard.putBoolean("Break Beam Sensor", self.breakBeam.get()) 299 wpilib.SmartDashboard.putNumber("Roller Sensor", self.rollerSensor) 300 # wpilib.SmartDashboard.putNumber("Intake Difference", self.intakeDifference) 301 # wpilib.SmartDashboard.putNumber("Remaining Rotations", self.remainingRotations) 302 # wpilib.SmartDashboard.putNumber("Intake Slowdown Position", self.intakeSlowdownPosition) 303 # wpilib.SmartDashboard.putNumber("Intake Ramped", self.intakeRamped) 304 # wpilib.SmartDashboard.putNumber("Intake Ramp Position", self.intakeRamp) 305 # wpilib.SmartDashboard.putBoolean("Intake Ramp Condition", self.intakeRampedCondition) 306 # wpilib.SmartDashboard.putNumberArray("Intake Positions", self.intakeMotorPositions) 307 # wpilib.SmartDashboard.putNumber("Intake Stowed", self.intakeStowed) 308 wpilib.SmartDashboard.putNumber("Roller Condition", self.rollerCondition) 309 wpilib.SmartDashboard.putBoolean("Roller Jam", self.jamDetected) 310 wpilib.SmartDashboard.putNumber("Actual Roller Velocity", self.rollerMotorEncoder.getVelocity()) 311 wpilib.SmartDashboard.putNumber("Baseline Detected Jam", self.baselineDetectedJam) 312 313 self.motorChecks() 314 # self.automaticRollerActivation() 315 # self.intakeSlowdown() 316 self.jamDetection()
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.