def __init__(self, robot):
super().__init__()
self.robot = robot
if RobotMap.motor_controllers == "talonsrx":
self._motors = [
CANTalon(RobotMap.motor_a_talon_id),
CANTalon(RobotMap.motor_b_talon_id),
CANTalon(RobotMap.motor_c_talon_id),
CANTalon(RobotMap.motor_d_talon_id)
]
for motor in self._motors:
motor.changeControlMode(RobotMap.drive_motor_mode)
elif RobotMap.motor_controllers == "victor":
self._motors = [
Victor(RobotMap.motor_a_pwm_id),
Victor(RobotMap.motor_b_pwm_id),
Victor(RobotMap.motor_c_pwm_id),
Victor(RobotMap.motor_d_pwm_id)
]
def __init__(self):
self.left_motor = Victor(0)
self.right_motor = Victor(1)
self.solenoid = DoubleSolenoid(2, 3)
self.lift_motor = Victor(4)
self.pot = AnalogPotentiometer(0)
# state
self._lift_state = None
self._spread_state = None
class GripperComponent(Events):
lift_positions = {
"up": 0.2,
# "middle": 197,
"down": 0.595
# "up": 2.2,
# "down": 4.0
}
class EVENTS(object):
gripper_started_moving = "gripper_started_moving"
gripper_started_moving_data = Command
def __init__(self):
Events.__init__(self)
self.left_motor = Victor(0)
self.right_motor = Victor(1)
self.solenoid = DoubleSolenoid(2, 3)
self.lift_motor = Victor(4)
self.pot = AnalogPotentiometer(0)
# state
self._lift_state = None
self._spread_state = None
# setup events
self._create_event(GripperComponent.EVENTS.gripper_started_moving)
def set_spread_state(self, spread: bool):
if spread:
self.solenoid.set(DoubleSolenoid.Value.kForward)
self._spread_state = True
else:
self.solenoid.set(DoubleSolenoid.Value.kReverse)
self._spread_state = False
def toggle_spread_state(self):
if self._spread_state is None:
self.set_spread_state(False)
else:
self.set_spread_state(not self._spread_state)
def set_motor_speeds(self, left: float, right: float):
self.left_motor.set(left)
self.right_motor.set(-right)
def set_lift_motor(self, speed):
print("grip_speed: {}".format(str(speed)))
self.lift_motor.set(speed)
def current_lift_state(self) -> str:
positions = [(key, position) for key, position in GripperComponent.lift_positions.items()]
return min(positions, key=lambda position: abs(self.pot.get() - position[1]))[0]
def __init__(self):
Events.__init__(self)
self.left_motor = Victor(0)
self.right_motor = Victor(1)
self.solenoid = DoubleSolenoid(2, 3)
self.lift_motor = Victor(4)
self.pot = AnalogPotentiometer(0)
# state
self._lift_state = None
self._spread_state = None
# setup events
self._create_event(GripperComponent.EVENTS.gripper_started_moving)
class GripperComponent:
lift_positions = {
"up": 0.13,
"down": 0.54,
# "up": 0.2,
# "down": 0.595
# "up": 2.2,
# "down": 4.0
}
def __init__(self):
self.left_motor = Victor(0)
self.right_motor = Victor(1)
self.solenoid = DoubleSolenoid(2, 3)
self.lift_motor = Victor(4)
self.pot = AnalogPotentiometer(0)
# state
self._lift_state = None
self._spread_state = None
def set_spread_state(self, spread: bool):
if spread:
self.solenoid.set(DoubleSolenoid.Value.kForward)
self._spread_state = True
else:
self.solenoid.set(DoubleSolenoid.Value.kReverse)
self._spread_state = False
def toggle_spread_state(self):
if self._spread_state is None:
self.set_spread_state(False)
else:
self.set_spread_state(not self._spread_state)
def set_motor_speeds(self, left: float, right: float):
self.left_motor.set(left)
self.right_motor.set(-right)
def set_lift_motor(self, speed):
self.lift_motor.set(speed)
def current_lift_state(self) -> str:
positions = [(key, position) for key, position in GripperComponent.lift_positions.items()]
return min(positions, key=lambda position: abs(self.pot.get() - position[1]))[0]
def __init__(self):
self.climb_motor_1 = Victor(2)
self.climb_motor_2 = Victor(5)
class ClimberComponent:
def __init__(self):
self.climb_motor_1 = Victor(2)
self.climb_motor_2 = Victor(5)
def climb(self):
self.climb_motor_1.set(-1)
self.climb_motor_2.set(-1)
def stop(self):
self.climb_motor_1.set(0)
self.climb_motor_2.set(0)
def release(self):
self.climb_motor_1.set(0.5)
self.climb_motor_2.set(0.5)
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)
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)