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