class Intake(Subsystem):
"""
cargo intake
"""
def __init__(self, port, robot):
super().__init__()
self.motor = Spark(port)
self.robot = robot
def set(self, speed):
"""
Sets motor speed (float val [-1,1])
"""
self.motor.set(speed)
def up(self):
self.set(1)
def down(self):
self.set(-1)
def stop(self):
self.set(0)
def initDefaultCommand(self):
self.setDefaultCommand(OperateIntake(self.robot))
class ClimbSubsystem(Subsystem):
def __init__(self, robot):
super().__init__("Climb")
self.robot = robot
self.winchMotor = Spark(robotmap.CLIMBWINCH)
self.armUpSolenoid = Solenoid(robotmap.CLIMBARM_UP)
self.armDownSolenoid = Solenoid(robotmap.CLIMBARM_DOWN)
self.lockCloseSolenoid = Solenoid(robotmap.CLIMBCLAMPLOCK_CLOSE)
self.lockOpenSolenoid = Solenoid(robotmap.CLIMBCLAMPLOCK_OPEN)
self.hookLimit = DigitalInput(robotmap.LIMIT_SWITCH_HOOK)
def pullyclimb(self, speed):
self.winchMotor.set(speed)
def armExtend(self):
self.armUpSolenoid.set(True)
self.armDownSolenoid.set(False)
def armRetract(self):
self.armUpSolenoid.set(False)
self.armDownSolenoid.set(True)
def clampLock(self):
self.lockCloseSolenoid.set(True)
self.lockOpenSolenoid.set(False)
def clampOpen(self):
self.lockCloseSolenoid.set(False)
self.lockOpenSolenoid.set(True)
def isHookPressed(self):
return self.hookLimit.get() == False
class Mechanisms(Subsystem):
"""
Subsystem with miscellaneous parts of the robot.
Includes many 'getters' and 'setters' for those different parts.
"""
def __init__(self):
# Hardware
self.stopper = DigitalInput(0)
self.crossbow = Spark(0)
self.intake = Spark(1)
self.gear_shift = DoubleSolenoid(0, 1)
self.intake_solenoid = DoubleSolenoid(2, 3)
# Quantities
self.intake_toggle = False
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kOff)
self.gear_shift.set(DoubleSolenoid.Value.kOff)
def set_crossbow(self, speed):
self.crossbow.set(speed)
def get_crossbow(self):
return self.crossbow.get()
def set_intake(self, speed):
self.intake.set(speed)
def pull_intake(self, setting):
self.intake_solenoid.set(setting)
def shift_gears(self, _setting):
self.gear_shift.set(_setting)
def get_stopper(self):
stopperState = self.stopper.get()
if not stopperState:
return True
elif stopperState:
return False
def initDefaultCommand(self):
self.setDefaultCommand(FollowJoystick())
class Winch(Subsystem):
def __init__(self):
super().__init__("Winch")
self.spark_winch = Spark(robotmap.spark_winch)
@classmethod
def setSpd(cls, spd_new):
robotmap.spd_winch = spd_new
def raiseWinch(self):
self.spark_winch.setInverted(True)
self.spark_winch.set(robotmap.spd_winch)
def stop(self):
self.spark_winch.set(0.0)
def initDefaultCommand(self):
pass
class Peripherals(Subsystem):
def __init__(self, robot):
super().__init__()
self.intake_spark = Spark(6)
self.control_panel_spark = Spark(5)
self.left_dispenser_gate = Servo(7)
self.right_dispenser_gate = Servo(8)
def run_intake(self, power=0):
self.intake_spark.set(power)
def run_spinner(self, power=0):
self.control_panel_spark.set(power)
def close_gate(self):
self.left_dispenser_gate.setAngle(120)
self.right_dispenser_gate.setAngle(135)
def open_gate(self):
self.left_dispenser_gate.setAngle(0)
self.right_dispenser_gate.setAngle(0)
def panel_clockwise(self, power):
print(power)
self.control_panel_spark.set(power)
def log(self):
pass
class ClawSubsystem(Subsystem):
def __init__(self, robot):
super().__init__("Claw")
self.robot = robot
self.leftMotor = Spark(robotmap.CLAWLEFT)
self.rightMotor = Spark(robotmap.CLAWRIGHT)
self.CLAW_OPEN = Solenoid(robotmap.PCM1_CANID,
robotmap.CLAW_OPEN_SOLENOID)
self.CLAW_CLOSE = Solenoid(robotmap.PCM1_CANID,
robotmap.CLAW_CLOSE_SOLENOID)
self.sensor = InfraredSensor(robotmap.INFRARED_SENSOR_CHANNEL)
self.close()
def initDefaultCommand(self):
self.setDefaultCommand(ObtainBoxContinuous(self.robot))
def open(self):
self.CLAW_OPEN.set(True)
self.CLAW_CLOSE.set(False)
def close(self):
self.CLAW_OPEN.set(False)
self.CLAW_CLOSE.set(True)
def setSpeed(self, pullSpeed):
#positive left speed is pull
self.leftMotor.set(pullSpeed)
self.rightMotor.set(-pullSpeed)
def boxIsClose(self):
return self.sensor.GetMedianVoltage() > 0.6
def boxIsReallyClose(self):
return self.sensor.GetMedianVoltage() > 2.6
class MyRobot(wpilib.TimedRobot):
"""Main robot class"""
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 1
rearLeftChannel = 2
frontRightChannel = 4
rearRightChannel = 3
ballGrabMotor = 1
ballTickler = 2
# The channel on the driver station that the joystick is connected to
lStickChannel = 0
rStickChannel = 1
# solenoids
PCM_CANID = 6
GEAR_ADJUST_RETRACT_SOLENOID = 0
GEAR_ADJUST_EXTEND_SOLENOID = 1
GEAR_DOOR_DROP_SOLENOID = 3
GEAR_DOOR_RAISE_SOLENOID = 2
GEAR_PUSHER_RETRACT_SOLENOID = 4
GEAR_PUSHER_EXTEND_SOLENOID = 5
BALL_DOOR_OPEN_SOLENOID = 6
BALL_DOOR_CLOSE_SOLENOID = 7
GRABBER_STATE = False
TICKLER_STATE = False
COMPRESSOR_STATE = False
GEAR_DOOR_STATE = False
GEAR_ADJUST_STATE = False
GEAR_PUSHER_STATE = False
AUTO_STATE = 0
WHEEL_CIRCUMFERENCE = 19 #inches
def robotInit(self):
self.timer = wpilib.Timer()
self.timer.start()
self.auto_timer = wpilib.Timer()
self.gyro = AHRS.create_spi()
self.gyro.reset()
self.compressor = wpilib.Compressor(self.PCM_CANID)
self.compressor.setClosedLoopControl(False)
#self.compressor.setClosedLoopControl(True)
#Solenoids galore
self.gearAdjustExtend = wpilib.Solenoid(
self.PCM_CANID, self.GEAR_ADJUST_EXTEND_SOLENOID)
self.gearAdjustRetract = wpilib.Solenoid(
self.PCM_CANID, self.GEAR_ADJUST_RETRACT_SOLENOID)
self.gearPusherExtend = wpilib.Solenoid(
self.PCM_CANID, self.GEAR_PUSHER_EXTEND_SOLENOID)
self.gearPusherRetract = wpilib.Solenoid(
self.PCM_CANID, self.GEAR_PUSHER_RETRACT_SOLENOID)
self.gearDoorDrop = wpilib.Solenoid(self.PCM_CANID,
self.GEAR_DOOR_DROP_SOLENOID)
self.gearDoorRaise = wpilib.Solenoid(self.PCM_CANID,
self.GEAR_DOOR_RAISE_SOLENOID)
self.ballDoorOpen = wpilib.Solenoid(self.PCM_CANID,
self.BALL_DOOR_OPEN_SOLENOID)
self.ballDoorClose = wpilib.Solenoid(self.PCM_CANID,
self.BALL_DOOR_CLOSE_SOLENOID)
"""Robot initialization function"""
self.frontLeftMotor = ctre.WPI_TalonSRX(self.frontLeftChannel)
self.rearLeftMotor = ctre.WPI_TalonSRX(self.rearLeftChannel)
self.frontRightMotor = ctre.WPI_TalonSRX(self.frontRightChannel)
self.rearRightMotor = ctre.WPI_TalonSRX(self.rearRightChannel)
self.tickler = Spark(self.ballTickler)
self.grabber = Victor(self.ballGrabMotor)
# invert the left side motors
self.frontLeftMotor.setInverted(True)
self.frontRightMotor.setInverted(True)
self.rearLeftMotor.setInverted(True)
self.rearRightMotor.setInverted(True)
self.talons = [
self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor,
self.rearRightMotor
]
for talon in self.talons:
talon.configNominalOutputForward(0.0, 25)
talon.configNominalOutputReverse(0.0, 25)
talon.configPeakOutputForward(1.0, 25)
talon.configPeakOutputReverse(-1.0, 25)
talon.enableVoltageCompensation(True)
talon.configVoltageCompSaturation(11.5, 25)
talon.configOpenLoopRamp(0.125, 25)
talon.config_kP(0, 0.375, 25)
talon.config_kI(0, 0.0, 25)
talon.config_kD(0, 0.0, 25)
talon.config_kF(0, 0.35, 25)
talon.selectProfileSlot(0, 0)
talon.configClosedLoopPeakOutput(0, 1.0, 25)
talon.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,
0, 25)
talon.configSelectedFeedbackCoefficient(1.0, 0, 25)
talon.set(ctre.ControlMode.PercentOutput, 0)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(False)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
def disabled(self):
"""Called when the robot is disabled"""
while self.isDisabled():
wpilib.Timer.delay(0.01)
def DriveStraight(self, distance):
pass
def autonomousInit(self):
self.auto_timer.start()
def autonomousPeriodic(self):
#self.auto_timer.delay(5)
while self.AUTO_STATE != 4:
"""
self.frontLeftMotor.set(ctre.ControlMode.PercentOutput, -0.4)
self.frontRightMotor.set(ctre.ControlMode.PercentOutput, 0.4)
self.rearLeftMotor.set(ctre.ControlMode.PercentOutput, -0.4)
self.rearRightMotor.set(ctre.ControlMode.PercentOutput, 0.4)
"""
if self.AUTO_STATE == 0:
self.drive.driveCartesian(0, .5, -.025)
if self.auto_timer.hasPeriodPassed(1.7):
self.AUTO_STATE = 1
#self.auto_timer.reset()
if self.AUTO_STATE == 1:
self.drive.driveCartesian(0, 0, 0)
if self.auto_timer.hasPeriodPassed(1.5):
self.AUTO_STATE = 2
if self.AUTO_STATE == 2:
self.drive.driveCartesian(0, -.5, 0.025)
if self.auto_timer.hasPeriodPassed(2):
self.AUTO_STATE = 3
if self.AUTO_STATE == 3:
self.drive.driveCartesian(0, 0, 0)
#for talon in self.talons:
#talon.stopMotor()
#self.logger.info("STAWP")
self.AUTO_STATE = 4
#self.drive.setExpiration(0.1)
#pass
def conditonAxis(self, axis, deadband, rate, expo, power, minimum,
maximum):
deadband = min(abs(deadband), 1)
rate = max(0.1, min(abs(rate), 10))
expo = max(0, min(abs(expo), 1))
power = max(1, min(abs(power), 10))
if axis > -deadband and axis < deadband:
axis = 0
else:
axis = rate * (math.copysign(1, axis) * ((abs(axis) - deadband) /
(1 - deadband)))
if expo > 0.01:
axis = ((axis * (abs(axis)**power) * expo) + (axis * (1 - expo)))
axis = max(min(axis, maximum), minimum)
return axis
def teleopInit(self):
self.compressor.setClosedLoopControl(False)
self.gyro.reset()
self.solenoid_timer = wpilib.Timer()
self.solenoid_timer.start()
for talon in self.talons:
talon.setQuadraturePosition(0)
self.GearAdjustRetract()
self.GearPusherRetract()
self.GearDoorDrop()
#Solenoid functions
def BallDoorOpen(self):
self.ballDoorOpen.set(True)
self.ballDoorClose.set(False)
self.tickler.set(-0.4)
def BallDoorClose(self):
self.ballDoorOpen.set(False)
self.ballDoorClose.set(True)
self.tickler.set(0.0)
def GearAdjustExtend(self):
self.gearAdjustExtend.set(True)
self.gearAdjustRetract.set(False)
def GearAdjustRetract(self):
self.gearAdjustExtend.set(False)
self.gearAdjustRetract.set(True)
def GearPusherExtend(self):
self.gearPusherExtend.set(True)
self.gearPusherRetract.set(False)
def GearPusherRetract(self):
self.gearPusherExtend.set(False)
self.gearPusherRetract.set(True)
def GearDoorDrop(self):
self.gearDoorDrop.set(True)
self.gearDoorRaise.set(False)
def GearDoorRaise(self):
self.gearDoorDrop.set(False)
self.gearDoorRaise.set(True)
#Ball Grab
def BallGrabStart(self):
self.grabber.set(-0.2)
def BallGrabStop(self):
self.grabber.set(0.0)
def activate(self, extend, retract, state):
self.solenoid_timer.reset()
self.logger.info(state)
if state:
extend()
else:
retract()
def teleopPeriodic(self):
"""Called when operation control mode is enabled"""
while self.isEnabled():
'''
self.logger.info("FrontLeft: " + str(self.frontLeftMotor.getMotorOutputVoltage()))
self.logger.info("RearLeft: " + str(self.rearLeftMotor.getMotorOutputVoltage()))
self.logger.info("FrontRight: " + str(self.frontRightMotor.getMotorOutputVoltage()))
self.logger.info("RearRight: " + str(self.rearRightMotor.getMotorOutputVoltage()))
'''
self.drive.driveCartesian(
-self.conditonAxis(self.lstick.getX(), 0.05, 0.85, 0.6, 1.5,
-1, 1),
self.conditonAxis(self.lstick.getY(), 0.05, 0.85, 0.6, 1.5, -1,
1),
-self.conditonAxis(self.rstick.getX(), 0.25, 0.85, 0.6, 0.5,
-1, 1)
#self.gyro.getYaw()
)
if self.solenoid_timer.hasPeriodPassed(0.5):
if JoystickButton(self.lstick, 3).get():
self.COMPRESSOR_STATE = not self.COMPRESSOR_STATE
self.activate(self.compressor.start, self.compressor.stop,
self.COMPRESSOR_STATE)
if JoystickButton(self.lstick, 2).get():
self.TICKLER_STATE = not self.TICKLER_STATE
self.activate(self.BallDoorOpen, self.BallDoorClose,
self.TICKLER_STATE)
if JoystickButton(self.lstick, 1).get():
self.GRABBER_STATE = not self.GRABBER_STATE
self.activate(self.BallGrabStart, self.BallGrabStop,
self.GRABBER_STATE)
#pilotstick 2
if JoystickButton(self.rstick, 3).get():
self.GEAR_DOOR_STATE = not self.GEAR_DOOR_STATE
self.activate(self.GearDoorRaise, self.GearDoorDrop,
self.GEAR_DOOR_STATE)
if JoystickButton(self.rstick, 5).get():
self.GEAR_ADJUST_STATE = not self.GEAR_ADJUST_STATE
self.activate(self.GearAdjustExtend,
self.GearAdjustRetract,
self.GEAR_ADJUST_STATE)
if JoystickButton(self.rstick, 4).get():
self.GEAR_PUSHER_STATE = not self.GEAR_PUSHER_STATE
self.activate(self.GearPusherExtend,
self.GearPusherRetract,
self.GEAR_PUSHER_STATE)
wpilib.Timer.delay(0.04)
class BallChute():
def __init__(self):
self.stateTimer = wpilib.Timer()
self.stateTimer.start()
self.ballRakeExtend = Solenoid(robotmap.PCM2_ID, robotmap.BALL_RAKE_EXTEND_SOLENOID)
self.ballRakeRetract = Solenoid(robotmap.PCM2_ID, robotmap.BALL_RAKE_RETRACT_SOLENOID)
self.ballHatchExtend = Solenoid(robotmap.PCM2_ID, robotmap.BALL_HATCH_EXTEND_SOLENOID)
self.ballHatchRetract = Solenoid(robotmap.PCM2_ID, robotmap.BALL_HATCH_RETRACT_SOLENOID)
self.ballTickler = Spark(robotmap.BALL_TICKLER_ID)
self.bottomIntake = Spark(robotmap.BOTTOM_INTAKE_ID)
self.rakeMotor = Talon(robotmap.RAKE_ID)
self.ballRakeExtend.set(False)
self.ballRakeRetract.set(True)
self.ballHatchExtend.set(False)
self.ballHatchRetract.set(True)
#-value so motors run that way we want them to
def RakeMotorStart(self, value):
self.rakeMotor.set(-value)
def RakeMotorStop(self):
self.rakeMotor.stopMotor()
def BottomMotorStart(self, value):
self.bottomIntake.set(-value)
def BottomMotorStop(self):
self.bottomIntake.stopMotor()
def BallTicklerStart(self, value):
self.ballTickler.set(value)
def BallTicklerStop(self):
self.ballTickler.stopMotor()
#Solenoid functions ot make things easier
def DeployRake(self):
self.ballRakeExtend.set(True)
self.ballRakeRetract.set(False)
def StowRake(self):
self.ballRakeExtend.set(False)
self.ballRakeRetract.set(True)
def OpenHatch(self):
self.ballHatchExtend.set(True)
self.ballHatchRetract.set(False)
def CloseHatch(self):
self.ballHatchExtend.set(False)
self.ballHatchRetract.set(True)
def Iterate(self, copilot: XBox):
if copilot.Y():
if copilot.Start():
self.BottomMotorStart(1)
elif copilot.X():
self.BottomMotorStart(-.5)
else:
self.BottomMotorStart(.5)
else:
self.BottomMotorStop()
if copilot.A():
elif copilot.X():
self.RakeMotorStart(-.5)
else:
self.RakeMotorStart(.5)
else:
class ColorWheel():
def __init__(self):
self.colorWheelExtend = Solenoid(robotmap.PCM_ID,
robotmap.COLOR_WHEEL_EXTEND_SOLENOID)
self.colorWheelRetract = Solenoid(
robotmap.PCM_ID, robotmap.COLOR_WHEEL_RETRACT_SOLENOID)
self.colorWheelMotor = Spark(robotmap.COLOR_WHEEL_ID)
self.RGBSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
self.colorWheelExtend.set(False)
self.colorWheelRetract.set(True)
def DeployWheelArm(self):
self.colorWheelExtend.set(True)
self.colorWheelRetract.set(False)
def StowWheelArm(self):
self.colorWheelExtend.set(False)
self.colorWheelRetract.set(True)
def Iterate(self, copilot: xboxcontroller.XBox):
self.color = self.RGBSensor.getColor()
self.ir = self.RGBSensor.getIR()
self.proxy = self.RGBSensor.getProximity(
) #proxy's still alive Jazz band
if self.color.red > 0.7 and self.color.red <= 1:
self.red = True
elif self.color.green > 0.7 and self.color.green <= 1:
self.green = True
elif self.color.blue > 0.7 and self.color.blue <= 1:
self.blue = True
else:
self.red = False
self.green = False
self.blue = False
#Boolean
wpilib.SmartDashboard.putBoolean("Red", self.red)
wpilib.SmartDashboard.putBoolean("Green", self.green)
wpilib.SmartDashboard.putBoolean("Blue", self.blue)
#Value
wpilib.SmartDashboard.putNumber("RedN", self.color.red)
wpilib.SmartDashboard.putNumber("BlueN", self.color.blue)
wpilib.SmartDashboard.putNumber("GreenN", self.color.green)
wpilib.SmartDashboard.putNumber("IR", self.ir)
wpilib.SmartDashboard.putNumber("Proxy", self.proxy)
#print(self.color)
if copilot.X():
self.colorWheelMotor.set(0.5)
elif copilot.B():
self.colorWheelMotor.set(-0.5)
else:
self.colorWheelMotor.stopMotor()
if copilot.getDPadRight():
self.DeployWheelArm()
elif copilot.getDPadLeft():
self.StowWheelArm()