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)