def __init__(self, pin): self._value = 0 # setup gpiozero to call increment on each when_activated encoder = DigitalInput(5) encoder.when_activated = self._increment encoder.when_deactivated = self._increment
def __init__(self, robot): super().__init__("Arm") self.robot = robot self.peakCurrentLimit = 30 self.PeaKDuration = 50 self.continuousLimit = 15 motor = {} for name in self.robot.RobotMap.motorMap.motors: motor[name] = self.robot.Creator.createMotor(self.robot.RobotMap.motorMap.motors[name]) self.motors = motor for name in self.motors: self.motors[name].setInverted(self.robot.RobotMap.motorMap.motors[name]['inverted']) # drive current limit self.motors[name].configPeakCurrentLimit(self.peakCurrentLimit, 10) self.motors[name].configPeakCurrentDuration(self.PeaKDuration, 10) self.motors[name].configContinuousCurrentLimit(self.continuousLimit, 10) self.motors[name].enableCurrentLimit(True) self.AEnc = Encoder(4, 5, False, Encoder.EncodingType.k4X) self.Zero = DigitalInput(6) self.kp = 0.00035 self.ki = 0.00000000001 self.kd = 0.0000001 self.ArmPID = PID(self.kp, self.ki, self.kd)
def __init__(self): self.left_fly = CANTalon(motor_map.left_fly_motor) self.right_fly = CANTalon(motor_map.right_fly_motor) self.intake_main = CANTalon(motor_map.intake_main_motor) self.intake_mecanum = Talon(motor_map.intake_mecanum_motor) self.ball_limit = DigitalInput(sensor_map.ball_limit) self.intake_mecanum.setInverted(True) self.left_fly.reverseOutput(True) self.left_fly.enableBrakeMode(False) self.right_fly.enableBrakeMode(False) self.left_fly.setControlMode(CANTalon.ControlMode.Speed) self.right_fly.setControlMode(CANTalon.ControlMode.Speed) self.left_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I, sensor_map.shoot_D, sensor_map.shoot_F, sensor_map.shoot_Izone, sensor_map.shoot_RR, sensor_map.shoot_Profile) self.right_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I, sensor_map.shoot_D, sensor_map.shoot_F, sensor_map.shoot_Izone, sensor_map.shoot_RR, sensor_map.shoot_Profile) self.left_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising) self.right_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising) self.left_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev) self.right_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev)
def __init__(self, robot): super().__init__("shooter") self.robot = robot self.counter = 0 # used for updating the log self.feed_forward = 3.5 # default volts to give the flywheel to get close to setpoint, optional # motor controllers self.sparkmax_flywheel = rev.CANSparkMax(5, rev.MotorType.kBrushless) self.spark_hood = Talon(5) self.spark_feed = Talon(7) # encoders and PID controllers self.hood_encoder = Encoder( 4, 5) # generic encoder - we'll have to install one on the 775 motor self.hood_encoder.setDistancePerPulse(1 / 1024) self.hood_controller = wpilib.controller.PIDController(Kp=0.005, Ki=0, Kd=0.0) self.hood_setpoint = 5 self.hood_controller.setSetpoint(self.hood_setpoint) self.flywheel_encoder = self.sparkmax_flywheel.getEncoder( ) # built-in to the sparkmax/neo self.flywheel_controller = self.sparkmax_flywheel.getPIDController( ) # built-in PID controller in the sparkmax # limit switches, use is TBD self.limit_low = DigitalInput(6) self.limit_high = DigitalInput(7)
def __init__(self, channel, module=1, reverse=False): """ Initializes the switch on some digital channel and module. Normally assumes switches are active low. """ super().__init__() self.s = DigitalInput(module, channel) self.reverse = reverse
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): super().__init__() self._l_motor = Talon(constants.motor_intake_l) self._r_motor = Talon(constants.motor_intake_r) self._intake_piston = Solenoid(constants.solenoid_intake) self._photosensor = DigitalInput(constants.far_photosensor) self._left_pwm = 0 self._right_pwm = 0 self._open = False
def compute_speed( self, speed: float, upper_switch: wpilib.DigitalInput, lower_switch: wpilib.DigitalInput, ) -> float: if not upper_switch.get() and speed > 0: return speed elif not lower_switch.get() and speed < 0: return speed else: return 0
def __init__(self): self.clockwise_limit_switch = DigitalInput( TURRET_CLOCKWISE_LIMIT_SWITCH) self.counterclockwise_limit_switch = DigitalInput( TURRET_COUNTERCLOCKWISE_LIMIT_SWITCH) self.turn_motor = SparkMax(TURRET_TURN_MOTOR) self.turn_pid = PIDController(0.4, 0.001, 0.02) self.shoot_motor_1 = Falcon(TURRET_SHOOT_MOTORS[0]) self.shoot_motor_2 = Falcon(TURRET_SHOOT_MOTORS[1]) self.timer = Timer() self.limelight = Limelight()
def initialize(self): timeout = 15 self.wristPowerSet = 0 SmartDashboard.putNumber("Wrist Power Set", self.wristPowerSet) self.maxVolts = 10 self.wristUpVolts = 4 self.wristDownVolts = -4 self.xbox = oi.getJoystick(2) self.out = 0 #below is the talon on the intake self.intake = Talon(map.intake) self.intake.setNeutralMode(2) self.intake.configVoltageCompSaturation(self.maxVolts) self.intake.configContinuousCurrentLimit(20, timeout) #20 Amps per motor self.intake.configPeakCurrentLimit( 30, timeout) #30 Amps during Peak Duration self.intake.configPeakCurrentDuration( 100, timeout) #Peak Current for max 100 ms self.intake.enableCurrentLimit(True) #Talon motor object created self.wrist = Talon(map.wrist) if not wpilib.RobotBase.isSimulation(): self.wrist.configFactoryDefault() self.wrist.setInverted(True) self.wrist.configVoltageCompSaturation(self.maxVolts) self.wrist.setNeutralMode(2) self.wrist.configClearPositionOnLimitF(True) self.wrist.configContinuousCurrentLimit(20, timeout) #20 Amps per motor self.wrist.configPeakCurrentLimit( 30, timeout) #30 Amps during Peak Duration self.wrist.configPeakCurrentDuration( 100, timeout) #Peak Current for max 100 ms self.wrist.enableCurrentLimit(True) self.bottomSwitch = DI(map.bottomSwitch) self.topSwitch = DI(map.topSwitch)
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
def __init__(self, robot): super().__init__() # Capslock because its a constant? # Added to current angle to account for max angle recalibration # Motors self.arm_motors = Arm_Motors() #limit = Back_Switch() #self.back_switch = limit.back_switch # Encoders #self.l_arm_encoder = My_Arm_Encoder(0,1) self.l_arm_encoder = My_Arm_Encoder(9, 10) self.limit_switch = DigitalInput(8) #self.l_arm_encoder = wpilib.Encoder(0, 1) # By empirical test self.max_click_rate = 318.0 # For getting encoder and accounting for limits and error # Arm starts in downward position self.arm_min_or_max = 0 #XXX experimentally tested self.max_ticks = 434 self.last_encoder = 0 # For creating the profile (arm profile) self.x_axis_ticks = [] # XXX angle max may be wrong self.angle_max = 150 # Slope of profile self.max_accel = 0.536 self.min_decel = 0.536 self.time = 10
def init(): """ Initialize switch objects. """ global gear_mech_switch gear_mech_switch = DigitalInput(robotmap.switches.gear_switch_channel)
def __init__(self, robot, name=None): super().__init__(name=name) self.robot = robot self.launchMin = self.k_launchMin self.launchMax = self.k_launchMax self.launchRange = self.launchMax - self.launchMin self.controlMode = None self.visionState = robot.visionState self.intakeLeftMotor = CANTalon(robot.map.k_IlLeftMotorId) self.intakeLeftMotor.changeControlMode(CANTalon.ControlMode.PercentVbus) self.intakeLeftMotor.reverseSensor(True) self.intakeLeftMotor.setSafetyEnabled(False) self.intakeRightMotor = CANTalon(robot.map.k_IlRightMotorId) self.intakeRightMotor.changeControlMode(CANTalon.ControlMode.PercentVbus) self.intakeRightMotor.setSafetyEnabled(False) self.aimMotor = CANTalon(robot.map.k_IlAimMotorId) # LiveWindow.addActuator("IntakeLauncher", "AimMotor", self.aimMotor) if self.aimMotor.isSensorPresent(CANTalon.FeedbackDevice.AnalogPot): self.aimMotor.setSafetyEnabled(False) self.aimMotor.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogPot) self.aimMotor.enableLimitSwitch(True, True) self.aimMotor.enableBrakeMode(True) self.aimMotor.setAllowableClosedLoopErr(5) self.aimMotor.configPeakOutpuVoltage(12.0, -12.0) self.aimMotor.setVoltageRampRate(150) # TODO: setPID if needed self.boulderSwitch = DigitalInput(robot.map.k_IlBoulderSwitchPort) self.launcherServoLeft = Servo(robot.map.k_IlServoLeftPort) self.launcherServoRight = Servo(robot.map.k_IlServoRightPort)
def init(): global drive_left_encoder, drive_right_encoder, elevator_encoder, gyro, back_photosensor, side_photosensor, pdp drive_left_encoder = Encoder(6, 7) drive_left_encoder.setDistancePerPulse(_drive_encoder_scale()) drive_right_encoder = Encoder(4, 5) drive_right_encoder.setDistancePerPulse(_drive_encoder_scale()) elevator_encoder = Encoder(0, 1, True) elevator_encoder.setDistancePerPulse(_elevator_encoder_scale()) gyro = Gyro(0) back_photosensor = DigitalInput(8) side_photosensor = DigitalInput(3) pdp = PowerDistributionPanel()
def __init__(self, Robot): self._intake = Robot.intake self._drive = Robot.drive self._ramp = 0 self._rampSpeed = 6 self._LiftMotorLeft = SyncGroup(LIFT_MOTOR_LEFT_IDS, WPI_VictorSPX) self._LiftMotorRight = SyncGroup(LIFT_MOTOR_RIGHT_IDS, WPI_VictorSPX) self._LiftMotorLeft.setInverted(True) self._liftEncoder = Encoder(LIFT_ENCODER_A, LIFT_ENCODER_B, False, Encoder.EncodingType.k4X) self._liftEncoder.setDistancePerPulse(1) self._liftHallaffect = DigitalInput(HALL_DIO) self.zeroEncoder() self._liftPID = MiniPID(*LIFT_PID) self._liftPID.setOutputLimits(-0.45, 1) self.disableSpeedLimit = False
def __init__(self, robot): super().__init__() self.robot = robot self.armMotor = CANTalon(4) self.wheelMotor = CANTalon(5) self.frontSwitch = DigitalInput(8) self.backSwitch = DigitalInput(9) self.comp = Compressor() self.comp.enabled() self.armMotor.enableBrakeMode(True) self.wheelMotor.enableBrakeMode(True) self.potentiometer = AnalogPotentiometer(3, 270, -193) #self.pidArm = PIDController(0.0, 0.0, 0.0, 0.0, self.potentiometer, self.armMotor, 0.02) self.position = 0
def __init__(self, channel, module=1, reverse=False): """ Initializes the switch on some digital channel and module. Normally assumes switches are active low. """ super().__init__() self.s = DigitalInput(channel) self.reverse = reverse
def __init__(self, robot): Subsystem.__init__(self, 'Conveyor') self.robot = robot self.blaser = DigitalInput(3) motors = {} self.map = self.robot.botMap for name in self.map.motorMap.motors: motors[name] = self.robot.Creator.createMotor( self.map.motorMap.motors[name]) self.cMotors = motors for name in self.cMotors: self.cMotors[name].setInverted( self.map.motorMap.motors[name]['inverted']) self.cMotors[name].setNeutralMode(ctre.NeutralMode.Coast)
class Intake(Component): def __init__(self): super().__init__() self._l_motor = Talon(constants.motor_intake_l) self._r_motor = Talon(constants.motor_intake_r) self._intake_piston = Solenoid(constants.solenoid_intake) self._photosensor = DigitalInput(constants.far_photosensor) self._left_pwm = 0 self._right_pwm = 0 self._open = False def update(self): self._l_motor.set(self._left_pwm) self._r_motor.set(self._right_pwm) self._intake_piston.set(not self._open) def spin(self, power, same_direction=False): self._left_pwm = power self._right_pwm = power * (1 if same_direction else -1) def intake_tote(self): power = .3 if not self._photosensor.get() else .75 self.spin(power) def intake_bin(self): power = .3 if not self._photosensor.get() else .65 self.spin(power) def open(self): self._open = True def close(self): self._open = False def stop(self): """Disables EVERYTHING. Only use in case of critical failure""" self._l_motor.set(0) self._r_motor.set(0)
def __init__(self): self.encoderUnit = 4096 self.gearRatio = 93.33 self.armSpeedMultiplier = 1 self.armBottomLimit = 5 self.armUpperLimit = 200 self.resetValue = 0 self.bottomLimitSwitch = DigitalInput(ArmLimitSwitch) self.armMotor1 = CANSparkMax(armBaseMotor1,MotorType.kBrushless) self.armMotor2 = CANSparkMax(armBaseMotor2,MotorType.kBrushless) self.armEncoder1 = self.armMotor1.getEncoder() self.armEncoder2 = self.armMotor2.getEncoder() self.currentArmPower = 0 self.isOverride = False
def __init__(self): super().__init__() self._motor = SyncGroup(Talon, constants.motor_elevator) self._position_encoder = Encoder(*constants.encoder_elevator) self._photosensor = DigitalInput(constants.photosensor) self._stabilizer_piston = Solenoid(constants.solenoid_dropper) self._position_encoder.setDistancePerPulse( (PITCH_DIAMETER * math.pi) / TICKS_PER_REVOLUTION) # Trajectory controlling stuff self._follower = TrajectoryFollower() self.assure_tote = CircularBuffer(5) self.auto_stacking = True # Do the dew self._tote_count = 0 # Keep track of totes! self._has_bin = False # Do we have a bin on top? self._new_stack = True # starting a new stack? self._tote_first = False # Override bin first to grab totes before anything else self._should_drop = False # Are we currently trying to get a bin ? self._close_stabilizer = True # Opens the stabilizer manually self.force_stack = False # manually actuates the elevator down and up self._follower.set_goal(Setpoints.BIN) # Base state self._follower_thread = Thread(target=self.update_follower) self._follower_thread.start() self._auton = False quickdebug.add_tunables(Setpoints, ["DROP", "STACK", "BIN", "TOTE", "FIRST_TOTE"]) quickdebug.add_printables( self, [('position', self._position_encoder.getDistance), ('photosensor', self._photosensor.get), "has_bin", "tote_count", "tote_first", "at_goal", "has_game_piece", "auto_stacking"])
class Conveyor(Subsystem): def __init__(self, robot): Subsystem.__init__(self, 'Conveyor') self.robot = robot self.blaser = DigitalInput(3) motors = {} self.map = self.robot.botMap for name in self.map.motorMap.motors: motors[name] = self.robot.Creator.createMotor( self.map.motorMap.motors[name]) self.cMotors = motors for name in self.cMotors: self.cMotors[name].setInverted( self.map.motorMap.motors[name]['inverted']) self.cMotors[name].setNeutralMode(ctre.NeutralMode.Coast) def log(self): wpilib.SmartDashboard.putNumber('Infared Value', self.blaser.get()) def set(self, pw): self.cMotors['conveyor1'].set(ctre.ControlMode.PercentOutput, pw) self.cMotors['conveyor2'].set(ctre.ControlMode.PercentOutput, pw) def stay(self, pw): self.cMotors['conveyor1'].set(ctre.ControlMode.PercentOutput, pw) self.cMotors['conveyor2'].set(ctre.ControlMode.PercentOutput, -pw) def getblaser(self): y = self.blaser.get() return y
def __init__(self, operator: OperatorControl): self.operator = operator self.motor = WPI_VictorSPX(7) self.encoder = Encoder(0, 1, False, Encoder.EncodingType.k4X) self.encoder.setIndexSource(2) self.limit = DigitalInput(4) self.dashboard = NetworkTables.getTable("SmartDashboard") self.totalValues = 2048 # * self.encoder.getEncodingScale() - 1 self.targetValue = 10 self.realValue = 0 self.allowedError = 10 self.hole = 4 self.holeOffset = 36 - 15 self.maxspeed = .25 self.minspeed = .1 self.speedmult = 1/300 self.speed = .1
class Switch(Sensor): """ Sensor wrapper for a switch. Has boolean attribute pressed. """ pressed = False def __init__(self, channel, module=1, reverse=False): """ Initializes the switch on some digital channel and module. Normally assumes switches are active low. """ super().__init__() self.s = DigitalInput(channel) self.reverse = reverse def poll(self): self.pressed = not self.s.get() ^ self.reverse
class Switch(Sensor): """ Sensor wrapper for a switch. Has boolean attribute pressed. """ pressed = False def __init__(self, channel, module=1, reverse=False): """ Initializes the switch on some digital channel and module. Normally assumes switches are active low. """ super().__init__() self.s = DigitalInput(module, channel) self.reverse = reverse def poll(self): self.pressed = not self.s.Get() ^ self.reverse
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
class Lift: def __init__(self, Robot): self._intake = Robot.intake self._drive = Robot.drive self._ramp = 0 self._rampSpeed = 6 self._LiftMotorLeft = SyncGroup(LIFT_MOTOR_LEFT_IDS, WPI_VictorSPX) self._LiftMotorRight = SyncGroup(LIFT_MOTOR_RIGHT_IDS, WPI_VictorSPX) self._LiftMotorLeft.setInverted(True) self._liftEncoder = Encoder(LIFT_ENCODER_A, LIFT_ENCODER_B, False, Encoder.EncodingType.k4X) self._liftEncoder.setDistancePerPulse(1) self._liftHallaffect = DigitalInput(HALL_DIO) self.zeroEncoder() self._liftPID = MiniPID(*LIFT_PID) self._liftPID.setOutputLimits(-0.45, 1) self.disableSpeedLimit = False def setSpeed(self, speed): self._LiftMotorLeft.set(speed) self._LiftMotorRight.set(speed) def rampSpeed(self, speed): self._ramp += (speed - self._ramp) / self._rampSpeed if False and speed > 0: #is max limit hit self._ramp = 0 self._LiftMotorLeft.set(self._ramp) self._LiftMotorRight.set(self._ramp) def setLoc(self, target): target = abs(target) if target > 1: target = 1 elif target <= 0.1: target = 0 SmartDashboard.putNumber("loc dfliusafusd", target) self._liftPID.setSetpoint(target) def periodic(self): if self.isBottom(): #zero switch is active zero encoder self.zeroEncoder() else: if self._intake.getSpeed() >= 0: self._intake.rampSpeed(0.3) scaledLift = self.getEncoderVal() / LIFT_HEIGHT speed = self._liftPID.getOutput(scaledLift) if not self.disableSpeedLimit: speedLimit = pow(0.25, scaledLift) self._drive.setSpeedLimit(speedLimit) else: self._drive.setSpeedLimit(1) self.rampSpeed(speed) def getEncoderVal(self): return abs(self._liftEncoder.getDistance()) def zeroEncoder(self): self._liftEncoder.reset() def isBottom(self): return not self._liftHallaffect.get()
class Shooter(Subsystem): # ----------------- INITIALIZATION ----------------------- def __init__(self, robot): super().__init__("shooter") self.robot = robot self.counter = 0 # used for updating the log self.feed_forward = 3.5 # default volts to give the flywheel to get close to setpoint, optional # motor controllers self.sparkmax_flywheel = rev.CANSparkMax(5, rev.MotorType.kBrushless) self.spark_hood = Talon(5) self.spark_feed = Talon(7) # encoders and PID controllers self.hood_encoder = Encoder( 4, 5) # generic encoder - we'll have to install one on the 775 motor self.hood_encoder.setDistancePerPulse(1 / 1024) self.hood_controller = wpilib.controller.PIDController(Kp=0.005, Ki=0, Kd=0.0) self.hood_setpoint = 5 self.hood_controller.setSetpoint(self.hood_setpoint) self.flywheel_encoder = self.sparkmax_flywheel.getEncoder( ) # built-in to the sparkmax/neo self.flywheel_controller = self.sparkmax_flywheel.getPIDController( ) # built-in PID controller in the sparkmax # limit switches, use is TBD self.limit_low = DigitalInput(6) self.limit_high = DigitalInput(7) def initDefaultCommand(self): """ When other commands aren't using the drivetrain, allow arcade drive with the joystick. """ #self.setDefaultCommand(ShooterHoodAxis(self.robot)) def set_flywheel(self, velocity): #self.flywheel_controller.setReference(velocity, rev.ControlType.kVelocity, 0, self.feed_forward) self.flywheel_controller.setReference(velocity, rev.ControlType.kSmartVelocity, 0) #self.flywheel_controller.setReference(velocity, rev.ControlType.kVelocity, 0) def stop_flywheel(self): self.flywheel_controller.setReference(0, rev.ControlType.kVoltage) def set_feed_motor(self, speed): self.spark_feed.set(speed) def stop_feed_motor(self): self.spark_feed.set(0) def set_hood_motor(self, power): power_limit = 0.2 if power > power_limit: new_power = power_limit elif power < -power_limit: new_power = -power_limit else: new_power = power self.spark_hood.set(new_power) def change_elevation( self, power ): # open loop approach - note they were wired to be false when contacted if power > 0 and self.limit_high.get(): self.set_hood_motor(power) elif power < 0 and self.limit_low.get(): self.set_hood_motor(power) else: self.set_hood_motor(0) def set_hood_setpoint(self, setpoint): self.hood_controller.setSetpoint(setpoint) def get_angle(self): elevation_minimum = 30 # degrees conversion_factor = 1 # encoder units to degrees # return elevation_minimum + conversion_factor * self.hood_encoder.getDistance() return self.hood_encoder.getDistance() def periodic(self) -> None: """Perform necessary periodic updates""" self.counter += 1 if self.counter % 5 == 0: # pass # ten per second updates SmartDashboard.putNumber('elevation', self.hood_encoder.getDistance()) SmartDashboard.putNumber('rpm', self.flywheel_encoder.getVelocity()) watch_axis = False if watch_axis: self.hood_scale = 0.2 self.hood_offset = 0.0 power = self.hood_scale * (self.robot.oi.stick.getRawAxis(2) - 0.5) + self.hood_offset self.robot.shooter.change_elevation(power) maintain_elevation = True if maintain_elevation: self.error = self.get_angle() - self.hood_setpoint pid_out = self.hood_controller.calculate(self.error) output = 0.03 + pid_out SmartDashboard.putNumber('El PID', pid_out) self.change_elevation(output) if self.counter % 50 == 0: SmartDashboard.putBoolean("hood_low", self.limit_low.get()) SmartDashboard.putBoolean("hood_high", self.limit_high.get())
class Arm(Subsystem): def __init__(self, robot): super().__init__("Arm") self.robot = robot self.peakCurrentLimit = 30 self.PeaKDuration = 50 self.continuousLimit = 15 motor = {} for name in self.robot.RobotMap.motorMap.motors: motor[name] = self.robot.Creator.createMotor(self.robot.RobotMap.motorMap.motors[name]) self.motors = motor for name in self.motors: self.motors[name].setInverted(self.robot.RobotMap.motorMap.motors[name]['inverted']) # drive current limit self.motors[name].configPeakCurrentLimit(self.peakCurrentLimit, 10) self.motors[name].configPeakCurrentDuration(self.PeaKDuration, 10) self.motors[name].configContinuousCurrentLimit(self.continuousLimit, 10) self.motors[name].enableCurrentLimit(True) self.AEnc = Encoder(4, 5, False, Encoder.EncodingType.k4X) self.Zero = DigitalInput(6) self.kp = 0.00035 self.ki = 0.00000000001 self.kd = 0.0000001 self.ArmPID = PID(self.kp, self.ki, self.kd) def log(self): wpilib.SmartDashboard.putNumber('armEnc', self.getHeight()) wpilib.SmartDashboard.putNumber('Zero', self.getZeroPos()) """ Get Functions """ def getHeight(self): # get encoder values return self.AEnc.get() def getZeroPos(self): # get zero position of arm return self.Zero.get() """ set functions """ def set(self, power): self.motors['RTArm'].set(ctre.ControlMode.PercentOutput, power) self.motors['LTArm'].set(ctre.ControlMode.PercentOutput, power) def stop(self): self.set(0) def resetHeight(self): self.AEnc.reset()
class IntakeLauncher(Subsystem): """A class to manage/control all aspects of shooting boulders. IntakeLauncher is comprised of: aimer: to raise & lower the mechanism for ball intake and shooting wheels: to suck or push the boulder launcher: to push boulder into the wheels during shooting limit switches: to detect if aimer is at an extreme position potentiometer: to measure our current aim position boulderSwitch: to detect if boulder is present Aiming is controlled via two modes 1: driver/interactive: aimMotor runs in VBUS mode to change angle 2: auto/vision control: aimMotor runs in closed-loop position mode to arrive at a designated position """ k_launchMin = 684.0 # manual calibration value k_launchMinDegrees = -11 # ditto (vestigial) k_launchMax = 1024.0 # manual calibration value k_launchMaxDegrees = 45 # ditto (vestigial) k_launchRange = k_launchMax - k_launchMin k_launchRangeDegrees = k_launchMaxDegrees - k_launchMinDegrees k_aimDegreesSlop = 2 # TODO: tune this k_intakeSpeed = -0.60 # pct vbux k_launchSpeedHigh = 1.0 # pct vbus k_launchSpeedLow = 0.7 k_launchSpeedZero = 0 k_servoLeftLaunchPosition = 0.45 # servo units k_servoRightLaunchPosition = 0.65 k_servoLeftNeutralPosition = 0.75 k_servoRightNeutralPosition = 0.4 k_aimMultiplier = 0.5 k_maxPotentiometerError = 5 # potentiometer units # launcher positions are measured as a percent of the range # of the potentiometer.. # intake: an angle approriate for intaking the boulder # neutral: an angle appropriate for traversing the lowbar # travel: an angle appropriate for traversing the rockwall, enableLimitSwitch # highgoal threshold: above which we shoot harder k_launchIntakeRatio = 0.0 k_launchTravelRatio = 0.51 k_launchNeutralRatio = 0.51 k_launchHighGoalThreshold = 0.69 def __init__(self, robot, name=None): super().__init__(name=name) self.robot = robot self.launchMin = self.k_launchMin self.launchMax = self.k_launchMax self.launchRange = self.launchMax - self.launchMin self.controlMode = None self.visionState = robot.visionState self.intakeLeftMotor = CANTalon(robot.map.k_IlLeftMotorId) self.intakeLeftMotor.changeControlMode(CANTalon.ControlMode.PercentVbus) self.intakeLeftMotor.reverseSensor(True) self.intakeLeftMotor.setSafetyEnabled(False) self.intakeRightMotor = CANTalon(robot.map.k_IlRightMotorId) self.intakeRightMotor.changeControlMode(CANTalon.ControlMode.PercentVbus) self.intakeRightMotor.setSafetyEnabled(False) self.aimMotor = CANTalon(robot.map.k_IlAimMotorId) # LiveWindow.addActuator("IntakeLauncher", "AimMotor", self.aimMotor) if self.aimMotor.isSensorPresent(CANTalon.FeedbackDevice.AnalogPot): self.aimMotor.setSafetyEnabled(False) self.aimMotor.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogPot) self.aimMotor.enableLimitSwitch(True, True) self.aimMotor.enableBrakeMode(True) self.aimMotor.setAllowableClosedLoopErr(5) self.aimMotor.configPeakOutpuVoltage(12.0, -12.0) self.aimMotor.setVoltageRampRate(150) # TODO: setPID if needed self.boulderSwitch = DigitalInput(robot.map.k_IlBoulderSwitchPort) self.launcherServoLeft = Servo(robot.map.k_IlServoLeftPort) self.launcherServoRight = Servo(robot.map.k_IlServoRightPort) def initDefaultCommand(self): self.setDefaultCommand(ILCmds.DriverControlCommand(self.robot)) def updateDashboard(self): pass def beginDriverControl(self): self.setControlMode("vbus") def driverControl(self, deltaX, deltaY): if self.robot.visionState.wantsControl(): self.trackVision() else: self.setControlMode("vbus") self.aimMotor.set(deltaY * self.k_aimMultiplier) def endDriverControl(self): self.aimMotor.disableControl() # ---------------------------------------------------------------------- def trackVision(self): if not self.visionState.TargetAcquired: return # hopefully someone is guiding the drivetrain to help acquire if not self.visionState.LauncherLockedOnTarget: if math.fabs(self.visionState.TargetY - self.getAngle()) < self.k_aimDegreesSlop: self.visionState.LauncherLockedOnTarget = True else: self.setPosition(self.degreesToTicks(self.TargetY)) elif self.visionState.DriveLockedOnTarget: # here we shoot and then leave AutoAimMode pass else: # do nothing... waiting form driveTrain to reach orientation pass def setControlMode(self, mode): if mode != self.controlMode: self.controlMode = mode if mode == "vbus": self.aimMotor.disableControl() self.aimMotor.changeControlMode(CANTalon.ControlMode.PercentVbus) self.aimMotor.enableControl() elif mode == "position": self.aimMotor.disableControl() self.aimMotor.changeControlMode(CANTalon.ControlMode.Position) self.aimMotor.enableControl() elif mode == "disabled": self.aimMotor.disableControl() else: self.robot.error("ignoring controlmode " + mode) self.controlMode = None self.aimMotor.disableControl() # launcher aim ------------------------------------------------------------- def getPosition(self): return self.aimMotor.getPosition() def getAngle(self): pos = self.getPosition() return self.ticksToDegress(pos) def setPosition(self, pos): self.setControlMode("position") self.aimMotor.set(pos) def isLauncherAtTop(self): return self.aimMotor.isFwdLimitSwitchClosed() def isLauncherAtBottom(self): return self.aimMotor.isRevLimitSwitchClosed() def isLauncherAtIntake(self): if self.k_intakeRatio == 0.0: return self.isLauncherAtBottom() else: return self.isLauncherNear(self.getIntakePosition()) def isLauncherAtNeutral(self): return self.isLauncherNear(self.getNeutralPosition()) def isLauncherAtTravel(self): return self.isLauncherNear(self.getTravelPosition()) def isLauncherNear(self, pos): return math.fabs(self.getPosition() - pos) < self.k_maxPotentiometerError def getIntakePosition(self): return self.getLauncherPositionFromRatio(self.k_launchIntakeRatio) def getNeutralPosition(self): return self.getLauncherPositionFromRatio(self.k_launchNeutralRatio) def getTravelPosition(self): return self.getLauncherPositionFromRatio(self.k_launchTravelRatio) def getLauncherPositionFromRatio(self, ratio): return self.launchMin + ratio * self.launchRange def degressToTicks(self, deg): ratio = (deg - self.k_launchMinDegrees) / self.k_launchRangeDegrees return self.k_launchMin + ratio * self.k_launchRange def ticksToDegrees(self, t): ratio = (t - self.k_launchMin) / self.k_launchRange return self.k_launchMinDegrees + ratio * self.k_launchRangeDegrees def calibratePotentiometer(self): if self.isLauncherAtBottom(): self.launchMin = self.getPosition() elif self.isLauncherAtTop(): self.launchMax = self.getPosition() self.launchRange = self.launchMax - self.launchMin # intake wheels controls --------------------------------------------------- def setWheelSpeedForLaunch(self): if self.isLauncherAtTop(): speed = self.k_launchSpeedHigh else: speed = self.k_launchSpeedLow self.setSpeed(speed) def setWheelSpeedForIntake(self): self.setSpeed(self.k_intakeSpeed) def stopWheels(self): self.setSpeed(k_launchSpeedZero) def setWheelSpeed(self, speed): self.intakeLeftMotor.set(speed) self.intakeRightMotor.set(-speed) # boulder and launcher servo controls -------------------------------------------------- def hasBoulder(self): return self.boulderSwitch.get() def activateLauncherServos(self): self.robot.info("activateLauncherServos at:" + self.getAimPosition()) self.launcherServoLeft.set(self.k_servoLeftLaunchPosition) self.launcherServoRight.set(self.k_servoRightLaunchPosition) def retractLauncherServos(self): self.launcherServoLeft.set(self.k_servoLeftNeutralPosition) self.launcherServoRight.set(self.k_servoRightNeutralPosition)
class Turret: ''' The object thats responsible for managing the shooter ''' def __init__(self): self.clockwise_limit_switch = DigitalInput( TURRET_CLOCKWISE_LIMIT_SWITCH) self.counterclockwise_limit_switch = DigitalInput( TURRET_COUNTERCLOCKWISE_LIMIT_SWITCH) self.turn_motor = SparkMax(TURRET_TURN_MOTOR) self.turn_pid = PIDController(0.4, 0.001, 0.02) self.shoot_motor_1 = Falcon(TURRET_SHOOT_MOTORS[0]) self.shoot_motor_2 = Falcon(TURRET_SHOOT_MOTORS[1]) self.timer = Timer() self.limelight = Limelight() def set_target_angle(self, angle): ''' Sets the target angle of the turret. This will use a PID to turn the turret to the target angle. ''' target_encoder = angle_to_encoder(angle) self.turn_pid.setSetpoint(target_encoder) def update(self): ''' This is used to continuously update the turret's event loop. All this manages as of now is the turrets PID controller. The shoot motor is constantly running at a low percentage until we need it. ''' motor_speed = self.turn_pid.calculate( self.limelight.get_target_screen_x()) if self.clockwise_limit_switch.get() and motor_speed < 0: self.turn_motor.set_percent_output(motor_speed) elif self.counterclockwise_limit_switch.get() and motor_speed > 0: self.turn_motor.set_percent_output(motor_speed) def shoot(self): ''' The wheel to shoot will rev up completely before balls start feeding in from the singulator. ''' # One of the motors will be reversed, so make sure the shoot motor has the correct ID! speed = self.shoot_motor_1.get_percent_output() if speed < 1: speed += 0.02 elif speed > 1: speed -= 0.02 self.shoot_motor_1.set_percent_output(speed) self.shoot_motor_2.set_percent_output(-speed) def idle(self): ''' Resets the motors back to their default state. ''' speed = self.shoot_motor_1.get_percent_output() if speed < 0.5: speed += 0.02 elif speed > 0.5: speed -= 0.02 self.shoot_motor_1.set_percent_output(speed) self.shoot_motor_2.set_percent_output(-speed) def is_full_speed(self): ''' Returns if the motor is at full speed or not. ''' self.timer.get() > 0.4
class arm(Component): #set up variables def __init__(self, robot): super().__init__() self.robot = robot self.armMotor = CANTalon(4) self.wheelMotor = CANTalon(5) self.frontSwitch = DigitalInput(8) self.backSwitch = DigitalInput(9) self.comp = Compressor() self.comp.enabled() self.armMotor.enableBrakeMode(True) self.wheelMotor.enableBrakeMode(True) self.potentiometer = AnalogPotentiometer(3, 270, -193) #self.pidArm = PIDController(0.0, 0.0, 0.0, 0.0, self.potentiometer, self.armMotor, 0.02) self.position = 0 def armAuto(self, upValue, downValue, target, rate=0.3): ''' if self.getPOT() <= target: self.armMotor.set(0) ''' if upValue == 1: self.armMotor.set(rate * -1) elif downValue == 1: self.armMotor.set(rate) else: self.armMotor.set(0) def armUpDown(self, left, right, controllerA, rate=0.3): rate2 = rate*1.75 if(self.backSwitch.get() == False or self.frontSwitch.get() == False): #Checking limit switches self.armMotor.set(0) if(left >= 0.75):#if tripped, disallow further movement # if(controllerA == True): # self.armMotor.set(rate2) # else: self.armMotor.set(rate) elif(right >= 0.75):#if tripped, disallow further movement # if(controllerA == True): # self.armMotor.set(-rate2) # else: self.armMotor.set(rate * -1) elif(left < 0.75 and right < 0.75): self.armMotor.set(0) def wheelSpin(self, speed): currentSpeed = 0 if (speed > 0.75): currentSpeed = -1 elif(speed < -0.75): currentSpeed = 1 self.wheelMotor.set(currentSpeed) def getPOT(self): return (self.potentiometer.get()*-1) def getBackSwitch(self): return self.backSwitch.get() def getFrontSwitch(self): return self.frontSwitch.get()
def initialize(self, robot): self.robot = robot self.xbox = oi.getJoystick(2) self.joystick0 = oi.getJoystick(0) if map.robotId == map.astroV1: '''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: '''IDS AND DIRECTIONS FOR V2''' self.backLift = Talon(map.frontLift) self.frontLift = Talon(map.backLift) self.frontLift.setInverted(False) self.backLift.setInverted(False) self.wheelLeft = Talon(map.wheelLeft) self.wheelRight = Talon(map.wheelRight) self.wheelRight.setInverted(True) self.wheelLeft.setInverted(False) 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.backBottomSensor) self.frontSwitch = DigitalInput(map.frontBottomSensor) self.upSwitch = DigitalInput(map.upSensor) self.MAX_ANGLE = 2 #degrees self.TARGET_ANGLE = 0 #degrees self.climbSpeed = 0.5 self.wheelSpeed = 0.5 self.backHold = -0.15 #holds back stationary if extended ADJUST** self.frontHold = -0.1 #holds front stationary if extended self.kP = 0.4 #proportional gain for angle to power ''' 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
class Climber(): def initialize(self, robot): self.robot = robot self.xbox = oi.getJoystick(2) self.joystick0 = oi.getJoystick(0) if map.robotId == map.astroV1: '''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: '''IDS AND DIRECTIONS FOR V2''' self.backLift = Talon(map.frontLift) self.frontLift = Talon(map.backLift) self.frontLift.setInverted(False) self.backLift.setInverted(False) self.wheelLeft = Talon(map.wheelLeft) self.wheelRight = Talon(map.wheelRight) self.wheelRight.setInverted(True) self.wheelLeft.setInverted(False) 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.backBottomSensor) self.frontSwitch = DigitalInput(map.frontBottomSensor) self.upSwitch = DigitalInput(map.upSensor) self.MAX_ANGLE = 2 #degrees self.TARGET_ANGLE = 0 #degrees self.climbSpeed = 0.5 self.wheelSpeed = 0.5 self.backHold = -0.15 #holds back stationary if extended ADJUST** self.frontHold = -0.1 #holds front stationary if extended self.kP = 0.4 #proportional gain for angle to power ''' 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 def periodic(self): if self.xbox.getRawButton(map.liftClimber): self.started = True deadband = 0.50 frontAxis = self.xbox.getRawAxis(map.liftFrontClimber) backAxis = self.xbox.getRawAxis(map.liftBackClimber) if abs(frontAxis) > deadband or abs(backAxis) > deadband: if frontAxis > deadband: self.extend("front") elif frontAxis < -deadband: self.retract("front") if backAxis > deadband: self.extend("back") elif backAxis < -deadband: self.retract("back") else: if self.xbox.getRawButton(map.lowerClimber) == True: self.retract("both") elif self.xbox.getRawButton(map.liftClimber) == True: self.extend("both") else: if self.xbox.getRawButton(map.driveForwardClimber): self.wheel("forward") elif self.xbox.getRawButton(map.driveBackwardClimber): self.wheel("backward") else: self.disable() def up(self): return not self.upSwitch.get() def getLean(self): if map.robotId == map.astroV1: return -1 * self.robot.drive.getRoll() else: return self.robot.drive.getPitch() def getCorrection(self): return (self.kP * -self.getLean()) def setSpeeds(self, back, front): self.backLift.set(back * self.climbSpeed) self.frontLift.set(front * self.climbSpeed) def retract(self, mode): correction = self.getCorrection() if mode == "front": self.setSpeeds(self.backHold, 1) elif mode == "back": self.setSpeeds(1, 0) elif mode == "both": self.setSpeeds(1 + correction, 1) else: self.setSpeeds(0, 0) def extend(self, mode): correction = self.getCorrection() if mode == "front": self.setSpeeds(correction, -1) elif mode == "back": self.setSpeeds(-1, 0) elif mode == "both": self.setSpeeds(-1 + correction, -1) elif self.up() == True: self.setSpeeds(self.backHold, self.frontHold) else: self.setSpeeds(0, 0) def wheel(self, direction): '''FORWARD MOVES ROBOT FORWARD, BACKWARD MOVES ROBOT BACKWARD''' if direction == "forward": self.wheelLeft.set(self.wheelSpeed) self.wheelRight.set(self.wheelSpeed) elif direction == "backward": self.wheelLeft.set(-1 * self.wheelSpeed) self.wheelRight.set(-1 * self.wheelSpeed) correction = self.getCorrection() self.setSpeeds(self.backHold + correction, 0) def stopClimb(self): self.setSpeeds(0, 0) def stopDrive(self): self.wheelLeft.set(0) self.wheelRight.set(0) def disable(self): self.stopClimb() self.stopDrive() def dashboardInit(self): SmartDashboard.putNumber("Climber kP", self.kP) SmartDashboard.putNumber("ClimbSpeed", self.climbSpeed) SmartDashboard.putNumber("BackHold", self.backHold) SmartDashboard.putNumber("FrontHold", self.frontHold) def dashboardPeriodic(self): self.climbSpeed = SmartDashboard.getNumber("ClimbSpeed", self.climbSpeed) self.kP = SmartDashboard.getNumber("Climber kP", self.kP) self.backHold = SmartDashboard.getNumber("BackHold", self.backHold) self.frontHold = SmartDashboard.getNumber("FrontHold", self.frontHold) SmartDashboard.putNumber("Lean", self.getLean())
class ArmSubsystem(Subsystem): def __init__(self): self.encoderUnit = 4096 self.gearRatio = 93.33 self.armSpeedMultiplier = 1 self.armBottomLimit = 5 self.armUpperLimit = 200 self.resetValue = 0 self.bottomLimitSwitch = DigitalInput(ArmLimitSwitch) self.armMotor1 = CANSparkMax(armBaseMotor1,MotorType.kBrushless) self.armMotor2 = CANSparkMax(armBaseMotor2,MotorType.kBrushless) self.armEncoder1 = self.armMotor1.getEncoder() self.armEncoder2 = self.armMotor2.getEncoder() self.currentArmPower = 0 self.isOverride = False #def initDefaultCommand(self): # self.setDefaultCommand(AnalogArmCommand()) def getDistanceTicks(self): return ((self.armEncoder1.getPosition() + self.armEncoder2.getPosition()) / 2) - resetValue def getArmMotor1Pos(self): return self.armEncoder1.getPosition def getArmMotor2Pos(self): return self.armEncoder2.getPosition def getRotationAngle(self): return (getDistanceTicks()/108) * -360 def updateBottomLimit(self): if not self.bottomLimitSwitch.get(): self.armBottomLimit = getRotationAngle() def isArmAtBottom(self): updateBottomLimit() if (getRotationAngle() >= (armBottomLimit - 2)) and (getRotationAngle() <= (armBottomLimit + 2)): return True else: return False #def isArmAtTop(self): #def getLimitSwitch(self): def setArmPower(self,power): if isArmAtBottom() and power > 0: self.currentArmPower = 0 else: self.currentArmPower = power def setArmPowerOverride(self,power): if isOverride: self.currentArmPower = power def updateOutputs(self): self.armMotor1.set(currentArmPower * armSpeedMultiplier) self.armMotor2.set(currentArmPower * armSpeedMultiplier) def getGravityCompensation(self): if getRotationAngle() <= 3: return 0 else: return math.sin(getRotationAngle() + 25)