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 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))
def robotInit(self):
# Note the lack of the camera server initialization here.
# Initializing drive motors
RLMotor = Spark(self.RLMotorChannel)
RRMotor = Spark(self.RRMotorChannel)
FLMotor = Spark(self.FLMotorChannel)
FRMotor = Spark(self.FRMotorChannel)
# Grouping drive motors into left and right.
self.Left = SpeedControllerGroup(RLMotor, FLMotor)
self.Right = SpeedControllerGroup(RRMotor, FRMotor)
# Initializing drive Joystick.
self.DriveStick = Joystick(self.DriveStickChannel)
# Initializing drive function.
self.drive = DifferentialDrive(self.Left, self.Right)
# Sets the right side motors to be inverted.
self.drive.setRightSideInverted(rightSideInverted=True)
# Turns the drive off after .1 seconds of inactivity.
self.drive.setExpiration(0.1)
# Components is a dictionary that contains any variables you want to put into it. All of the variables put into
# components dictionary is given to the autonomous modes.
self.components = {"drive": self.drive}
# Sets up the autonomous mode selector by telling it where the autonomous modes are at and what the autonomous
# modes should inherit.
self.automodes = autonomous.AutonomousModeSelector(
"Sim_Autonomous", self.components)
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 __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 __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 __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 __init__(self):
super().__init__("Chassis")
self.spark_L1 = Spark(robotmap.spark_L1)
self.spark_L2 = Spark(robotmap.spark_L2)
self.spark_R1 = Spark(robotmap.spark_R1)
self.spark_R2 = Spark(robotmap.spark_R2)
self.spark_group_L = SpeedControllerGroup(self.spark_L1, self.spark_L2)
self.spark_group_R = SpeedControllerGroup(self.spark_R1, self.spark_R2)
self.drive = DifferentialDrive(self.spark_group_L, self.spark_group_R)
self.gyro = ADXRS450_Gyro(robotmap.gyro)
self.encoder_L = Encoder(0, 1)
self.encoder_R = Encoder(2, 3)
self.dist_pulse_L = pi * 6 / 2048
self.dist_pulse_R = pi * 6 / 425
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())
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)
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
def __init__(self, robot, l_f_port=0, l_r_port=1, r_f_port=2, r_r_port=3):
"""
Initialize ports for bot
var naming scheme: (side)_(position)_(object), ex: l_f_port is left_front_port
"""
super().__init__()
self.robot = robot
# Motors on Left Side
self.l_f_motor = Spark(l_f_port)
self.l_r_motor = Spark(l_r_port)
# Motors on Right Side
self.r_f_motor = Spark(r_f_port)
self.r_r_motor = Spark(r_r_port)
# Motor groups
self.l_group = SpeedControllerGroup(self.l_f_motor, self.l_r_motor)
self.r_group = SpeedControllerGroup(self.r_f_motor, self.r_r_motor)
self.drive = DifferentialDrive(self.l_group, self.r_group)
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 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 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 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()
def robotInit(self):
#self.neo: CANSparkMax = CANSparkMax(5, rev.MotorType.kBrushless)
self.neo: Spark = Spark(0)
sd.putNumber("Neo Power", 0.5)
def __init__(self, port, robot):
super().__init__()
self.motor = Spark(port)
self.robot = robot
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:
def __init__(self):
super().__init__("Winch")
self.spark_winch = Spark(robotmap.spark_winch)
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 __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 initialize(self, robot):
self.state = -1
self.robot = robot
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
self.usingNeo = True
self.frontRetractStart = 0
self.wheelsStart = 0
self.wheelsStart2 = 0
if self.usingNeo:
# NOTE: If using Spark Max in PWM mode to control Neo brushless
# motors you MUST configure the speed controllers manually
# using a USB cable and the Spark Max client software!
self.frontLift: Spark = Spark(map.frontLiftPwm)
self.backLift: Spark = Spark(map.backLiftPwm)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
if map.robotId == map.astroV1:
if not self.usingNeo:
'''IDS AND DIRECTIONS FOR V1'''
self.backLift = Talon(map.backLift)
self.frontLift = Talon(map.frontLift)
self.frontLift.setInverted(True)
self.backLift.setInverted(True)
self.wheelLeft = Victor(map.wheelLeft)
self.wheelRight = Victor(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(True)
else:
if not self.usingNeo:
'''IDS AND DIRECTIONS FOR V2'''
self.backLift = Talon(map.frontLift)
self.frontLift = Talon(map.backLift)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
self.backLift.setNeutralMode(2)
self.frontLift.setNeutralMode(2)
self.wheelLeft = Talon(map.wheelLeft)
self.wheelRight = Talon(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(False)
if not self.usingNeo:
for motor in [self.backLift, self.frontLift]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.configContinuousCurrentLimit(30, 0) #Amps per motor
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(10,
0) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
for motor in [self.wheelLeft, self.wheelRight]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.setNeutralMode(2)
self.backSwitch = DigitalInput(map.backFloor)
self.frontSwitch = DigitalInput(map.frontFloor)
self.switchTopBack = DigitalInput(map.backTopSensor)
self.switchTopFront = DigitalInput(map.frontTopSensor)
self.switchBottomBack = DigitalInput(map.backBottomSensor)
self.switchBottomFront = DigitalInput(map.frontBottomSensor)
self.MAX_ANGLE = 2 #degrees
self.TARGET_ANGLE = -1 #degrees
self.climbSpeed = 0.9
self.wheelSpeed = 0.9
self.backHold = -0.10 #holds back stationary if extended ADJUST**
self.frontHold = -0.10 #holds front stationary if extended
self.kP = 0.35 #proportional gain for angle to power
self.autoClimbIndicator = False
'''
NEGATIVE POWER TO ELEVATOR LIFTS ROBOT, LOWERS LEGS
POSITIVE POWER TO ELEVATOR LOWERS ROBOT, LIFTS LEGS
NEGATIVE POWER TO WHEELS MOVES ROBOT BACKWARDS
POSITIVE POWER TO WHEELS MOVES ROBOT FORWARD
'''
self.started = False
