class Foo(object): robotTime = ntproperty('/SmartDashboard/robotTime', 0, writeDefault=False) dsTime = ntproperty('/SmartDashboard/dsTime', 0, writeDefault=True) testArray = ntproperty('/SmartDashboard/testArray', [1, 2, 3], writeDefault=True)
class Client(object): '''Connect to the Network table and setup properties''' Object = ntproperty("/hephestus/object_data", 'N/A') target_type = ntproperty("/hephestus/target_type", 'N/A') target_size = ntproperty("/hephestus/target_size", 00.00) target_offset = ntproperty("/hephestus/target_offset", 00.00)
class Gimbal: """ This class allows for the camera Gimble to be set through NetworkTables """ # Yaw = left/right, pitch = up/down gimbal_yaw = wpilib.Servo gimbal_pitch = wpilib.Servo yaw = ntproperty('/camera/gimbal/yaw', 0) pitch = ntproperty('/camera/gimbal/pitch', 0.52) def setup(self): self.sd = NetworkTable.getTable('SmartDashboard') def on_enable(self): pass def execute(self): """ Repeating code. """ self.gimbal_yaw.set(self.yaw) self.gimbal_pitch.set(self.pitch) def update_sd(self, name): """ Put refreshed values to SmartDashboard. """ pass
class SomeClient(object): """Demonstrates an object with magic networktables properties""" robotTime = ntproperty("/SmartDashboard/robotTime", 0, writeDefault=False) dsTime = ntproperty("/ADifferentTable/dsTime", 0)
class Client(object): '''Connect to the Network table and setup properties''' Inter = ntproperty("/hephestus/Intersection", 'N/A') Vect = ntproperty("/hephestus/Vector", 'N/A') luminary = ntproperty("/hephestus/lights", False)
class Foo(object): robotTime = ntproperty( "/SmartDashboard/robotTime", 0, writeDefault=False, inst=nt ) dsTime = ntproperty("/SmartDashboard/dsTime", 0, writeDefault=True, inst=nt) testArray = ntproperty( "/SmartDashboard/testArray", [1, 2, 3], writeDefault=True, inst=nt )
class PanelSpinner(StateMachine): control_panel: ControlPanel isSpinningPosition = ntproperty('/Controllers/panelSpinner/isSpinningPosition', False) isSpinningRotation = ntproperty('/Controllers/panelSpinner/isSpinningRotation', False) def on_enable(self): self.resting_timestamp = None super().on_enable() def spin_to(self): if self.control_panel.fms_color is not None: self.engage('positionControl') self.isSpinningPosition = True else: self.engage('rotationControl') self.isSpinningRotation = True # First here doesn't matter because we use the argument form of engage @state(first=True, must_finish=True) def rotationControl(self, initial_call): if initial_call: self.rotations = 0 self.last_color = self.control_panel.detected_color if self.control_panel.detected_color != self.last_color: self.rotations += 1 self.last_color = self.control_panel.detected_color self.control_panel.spin(0.27) if self.rotations >= 28: self.isSpinningRotation = False self.done() @state(must_finish=True) def positionControl(self, state_tm): if self.control_panel.turn_to_color is None or self.control_panel.detected_color is None: return if self.control_panel.detected_color != self.control_panel.turn_to_color: # print(f'Detected: {self.detected_color.red}, {self.detected_color.green}, {self.detected_color.blue} Towards: {self.turn_to_color.red}, {self.turn_to_color.green}, {self.turn_to_color.blue}') direction = math.copysign(1, self.control_panel.colors.index( self.control_panel.detected_color) - self.control_panel.colors.index(self.control_panel.turn_to_color)) self.control_panel.spin(direction * 0.2) self.resting_timestamp = None else: if self.resting_timestamp is None: self.resting_timestamp = state_tm # Wait on the correct color for one second if state_tm - self.resting_timestamp < 1: self.control_panel.spin(0) else: self.done() self.isSpinningPosition = False
class NTVars: sendError = ntproperty( "/visionProcessing/error", False, writeDefault=True, doc="Returns True if there is currently an error.", ) sendErrorMessage = ntproperty( "/visionProcessing/errorMessage", "No current error message.", writeDefault=True, doc="This is the current vision error message from the raspberry pi.", ) xValue = ntproperty( "/visionProcessing/targetInfo/contoursReport/x", [0.0, 0.0], writeDefault=True, doc="This is the x value from the gripPipeline.", ) yValue = ntproperty( "/visionProcessing/targetInfo/contoursReport/y", [0.0, 0.0], writeDefault=True, doc="This is the y value from the gripPipeline.", ) wValue = ntproperty( "/visionProcessing/targetInfo/contoursReport/width", [0.0, 0.0], writeDefault=True, doc="This is the width value from the gripPipeline.", ) hValue = ntproperty( "/visionProcessing/targetInfo/contoursReport/height", [0.0, 0.0], writeDefault=True, doc="This is the height value from the gripPipeline.", ) areaValue = ntproperty( "/visionProcessing/targetInfo/contoursReport/area", [0.0, 0.0], writeDefault=True, doc="This is the area value from the gripPipeline.", ) solidValue = ntproperty( "/visionProcessing/targetInfo/contoursReport/solidity", [0.0, 0.0], writeDefault=True, doc="This is the solidity value from the gripPipeline.", ) ratioValue = ntproperty( "/visionProcessing/targetInfo/contoursReport/ratio", [0.0, 0.0], writeDefault=True, doc="This is the ratio value from the gripPipeline.", )
class Odometry: kinematics: DifferentialDriveKinematics navx: navx.AHRS left_encoder: CANEncoder right_encoder: CANEncoder left_distance = ntproperty('/left distance/', 0) right_distance = ntproperty('/right distance/', 0) def getAngle(self): """Return the navx angle with counter-clockwise as positive""" return -self.navx.getAngle() def setup(self): self.odometry = DifferentialDriveOdometry( Rotation2d(math.radians(self.getAngle()))) def get_pose(self) -> Pose2d: return self.odometry.getPose() @feedback def get_pose_string(self) -> str: pose = self.get_pose() return f'({pose.translation().X()}, {pose.translation().Y()}, {pose.rotation()}' def get_distance(self, left=True): if left: return self.left_distance else: return -self.right_distance def reset(self, new_pose: Pose2d = Pose2d()): self.odometry.resetPosition(new_pose, Rotation2d.fromDegrees(self.getAngle())) self.left_encoder.setPosition(0) self.right_encoder.setPosition(0) @property def left_rate(self): return self.left_encoder.getVelocity() @property def right_rate(self): return -self.right_encoder.getVelocity() def execute(self): self.odometry.update(Rotation2d(math.radians(self.getAngle())), self.left_encoder.getPosition(), -self.right_encoder.getPosition()) self.left_distance = self.left_encoder.getPosition() self.right_distance = -self.right_encoder.getPosition()
class Limelight(): YAW_ANGLE = ntproperty('/limelight/tx', 0) PITCH_ANGLE = ntproperty('/limelight/ty', 0) # change with new robot; UNIT = inches CAMERA_HEIGHT = 43 TARGET_HEIGHT = 98.25 # UNIT = degrees CAMERA_ANGLE = 0 def findDistance(self): if self.YAW_ANGLE == 0 or self.PITCH_ANGLE == 0: return 0 else: distance = (self.TARGET_HEIGHT / self.CAMERA_HEIGHT ) / math.tan(self.CAMERA_ANGLE + self.TY) return distance
class PhysicsEngine: """ Simulates a motor moving something that strikes two limit switches, one on each end of the track. Obviously, this is not particularly realistic, but it's good enough to illustrate the point """ # array of (found, timestamp, angle) target = ntproperty("/camera/target", (0.0, float("inf"), 0.0)) def __init__(self, physics_controller): """ :param physics_controller: `pyfrc.physics.core.PhysicsInterface` object to communicate simulation effects to """ self.physics_controller = physics_controller self.position = 0 self.physics_controller.add_device_gyro_channel("adxrs450_spi_0_angle") targets = [ # right VisionSim.Target(15, 13, 250, 0), # middle VisionSim.Target(16.5, 15.5, 295, 65), # left VisionSim.Target(15, 18, 0, 110), ] self.vision = VisionSim(targets, 61.0, 1.5, 15, 15, physics_controller=physics_controller) def update_sim(self, hal_data, now, tm_diff): """ Called when the simulation parameters for the program need to be updated. :param now: The current time as a float :param tm_diff: The amount of time that has passed since the last time that this function was called """ # Simulate the drivetrain l_motor = hal_data["pwm"][0]["value"] r_motor = hal_data["pwm"][1]["value"] speed, rotation = drivetrains.two_motor_drivetrain(l_motor, r_motor) self.physics_controller.drive(speed, rotation, tm_diff) x, y, angle = self.physics_controller.get_position() data = self.vision.compute(now, x, y, angle) if data is not None: self.target = data[0][:3]
class Nt3656(object): """Read / Write access to nettables.""" writeDefault = False hsv_h_lo = ntproperty('/Preferences/hsv_h_lo', 53.0, writeDefault=writeDefault) hsv_h_hi = ntproperty('/Preferences/hsv_h_hi', 103.0, writeDefault=writeDefault) hsv_s_lo = ntproperty('/Preferences/hsv_s_lo', 0.0, writeDefault=writeDefault) hsv_s_hi = ntproperty('/Preferences/hsv_s_hi', 255.0, writeDefault=writeDefault) hsv_v_lo = ntproperty('/Preferences/hsv_v_lo', 100.0, writeDefault=writeDefault) hsv_v_hi = ntproperty('/Preferences/hsv_v_hi', 255.0, writeDefault=writeDefault) vis_cam_brightness = ntproperty('/Preferences/vis_cam_brightness', 30, writeDefault=True) def __init__(self): self.sd = NetworkTables.getTable('SmartDashboard') self.prefs = NetworkTables.getTable('Preferences') pass def isConnected(self): return NetworkTables.isConnected()
class PhysicsEngine(object): ''' Simulates a motor moving something that strikes two limit switches, one on each end of the track. Obviously, this is not particularly realistic, but it's good enough to illustrate the point ''' # array of (found, timestamp, angle) target = ntproperty('/camera/target', (0.0, float('inf'), 0.0)) def __init__(self, physics_controller): ''' :param physics_controller: `pyfrc.physics.core.PhysicsInterface` object to communicate simulation effects to ''' self.physics_controller = physics_controller self.position = 0 self.physics_controller.add_device_gyro_channel('adxrs450_spi_0_angle') targets = [ # right VisionSim.Target(15, 13, 250, 0), # middle VisionSim.Target(16.5, 15.5, 295, 65), # left VisionSim.Target(15, 18, 0, 110) ] self.vision = VisionSim(targets, 61.0, 1.5, 15, 15, physics_controller=physics_controller) def update_sim(self, hal_data, now, tm_diff): ''' Called when the simulation parameters for the program need to be updated. :param now: The current time as a float :param tm_diff: The amount of time that has passed since the last time that this function was called ''' # Simulate the drivetrain l_motor = hal_data['pwm'][0]['value'] r_motor = hal_data['pwm'][1]['value'] #global l_motor = 0.5 #global r_motor = 0.5 #import pdb; set_trace(); #print("hal_data", l_motor) speed, rotation = drivetrains.two_motor_drivetrain(l_motor, r_motor) self.physics_controller.drive(speed, rotation, tm_diff) #print("update_sim left right forward rotate", l_motor, r_motor, speed, rotation) x, y, angle = self.physics_controller.get_position() #import pdb;pdb.set_trace() data = self.vision.compute(now, x, y, angle) if data is not None: self.target = data[0][:3]
class Nt3656(object): """Read / Write access to nettables.""" writeDefault = False loc = '/SmartDashboard' hsv_h_lo = ntproperty('%s/hsv_h_lo' % loc, 50.0, writeDefault=writeDefault) #changed from 20 hsv_h_hi = ntproperty('%s/hsv_h_hi' % loc, 103.0, writeDefault=writeDefault) #changed from 50 hsv_s_lo = ntproperty('%s/hsv_s_lo' % loc, 0.0, writeDefault=writeDefault) hsv_s_hi = ntproperty('%s/hsv_s_hi' % loc, 230.0, writeDefault=writeDefault) hsv_v_lo = ntproperty('%s/hsv_v_lo' % loc, 20, writeDefault=writeDefault) #changed from -10.1 hsv_v_hi = ntproperty('%s/hsv_v_hi' % loc, 255.0, writeDefault=writeDefault) vis_cam_brightness = ntproperty('%s/vis_cam_brightness' % loc, 100, writeDefault=writeDefault) yellowbox_mode = ntproperty('/SmartDashboard/yellowbox_mode', "off", writeDefault=writeDefault) #shutter_speed = ntproperty( '/Preferences/vis_cam_shutter_speed', 5000, writeDefault = writeDefault ) # 5000 auton_mode = ntproperty( '%s/auton_mode' % loc, 0, writeDefault=False) # The rio should write this value def __init__(self): self.sd = NetworkTables.getTable('SmartDashboard') self.prefs = NetworkTables.getTable('Preferences') #self.banana() pass def banana(self): # detect a banana self.hsv_h_lo = 190 self.hsv_h_hi = 255 self.hsv_s_lo = 150 self.hsv_s_hi = 190 self.hsv_v_lo = 200 self.hsv_v_hi = 255 def isConnected(self): return NetworkTables.isConnected()
class hpMiniPython(object): findShipBayLeft = ntproperty('/Vision/findShipBayLeft', False) findShipBayCenter = ntproperty('/Vision/findShipBayCenter', False) findShipBayRight = ntproperty('/Vision/findShipBayRight', False) #angleToTarget = ntproperty('/PathFinder/angleToTarget', 0) #distanceToTarget = ntproperty('/PathFinder/distanceToTarget', 0) vectorToTarget = ntproperty('/PathFinder/vectorToTarget', [0, 0]) gameStartSeconds = ntproperty('/Vision/gameStartSeconds', 0) secondsSinceStart = ntproperty('/PathFinder/secondsSinceStart', 0) FrameCounter = ntproperty('/Vision/FrameCounter', 0)
class VisionCamera(wpilib.interfaces.PIDSource): target = ntproperty("/camera/target", (0, 0.0, 0.0)) #found, angle and distance def __init__(self, addr=4): self.i2c = wpilib.I2C(wpilib.I2C.Port.kOnboard, addr) self.msg_length = 8 self.data = [] self.offset = 0 self.distance = 0 def poll(self): res = [] try: res = self.i2c.readOnly(self.msg_length) except Exception as e: self.data = [0,0] pass try: self.data = struct.unpack('<ii', bytearray(res)) except: self.data = [0,0] self.offset = self.data[0]+60 self.distance = self.data[1] # print('got data: %i' % self.data) print('got offset: %i' % self.offset) print('got distance: %i' % self.distance) def pidGet(self): if wpilib.RobotBase.isSimulation(): found, offset, distance = self.target #print('offset: %s, dist:%s'%( offset, distance)) return offset else: self.poll() return self.offset def getPIDSourceType(self): return 'crosstrack' def setPIDSourceType(self, source_type): return True
class Intake: intake_motor: WPI_VictorSPX intake_motor_lower: CANSparkMax intake_switch: wpilib.DigitalInput intake_solenoid: wpilib.DoubleSolenoid balls_collected = ntproperty("/components/intake/ballsCollected", 0) speed = will_reset_to(0.0) speed_lower = will_reset_to(0.0) solenoid_state = will_reset_to(wpilib.DoubleSolenoid.Value.kForward) previous_limit = False def spin(self, speed, speed_lower: float): self.spin_inside(speed) self.spin_lower(speed_lower) def spin_lower(self, speed_lower: float): self.speed_lower = speed_lower def spin_inside(self, speed: float): self.speed = speed def extend(self): self.set_solenoid(True) def retract(self): self.set_solenoid(False) def set_solenoid(self, extended: bool): self.solenoid_state = wpilib.DoubleSolenoid.Value.kReverse if extended else wpilib.DoubleSolenoid.Value.kForward def execute(self): self.intake_motor.set(self.speed) self.intake_motor_lower.set(self.speed_lower) if self.intake_switch.get(): if self.intake_switch.get() is not self.previous_limit: self.balls_collected += 1 self.previous_limit = self.intake_switch.get() self.intake_solenoid.set(self.solenoid_state)
class Robot(magicbot.MagicRobot): # Order of components determines order that execute methods are run # State Machines panel_spinner: PanelSpinner auto_launcher: AutoShoot # Other components align: Align odometry: Odometry follower: Follower intake: Intake control_panel: ControlPanel launcher: Launcher drive: Drive flipped = tunable(False) ntSolenoid_state = ntproperty("/robot/ntSolenoid_state", 0) ds_velocity_setpoint = ntproperty('/components/launcher/DS_VEL_SETPOINT', 2100) limelight_stream_mode = ntproperty('/limelight/stream', 2) WHEEL_DIAMETER = 0.1524 # Units: Meters ENCODER_PULSES_PER_REV = 2048 # Ignore quadrature decoding (4x, 8192) LAUNCHER_GEARING = 1 # No gearing since encoder is on shaft GEARING = 7.56 # 7.56:1 gear ratio def createObjects(self): # Joysticks self.joystick_left = wpilib.Joystick(0) self.joystick_right = wpilib.Joystick(1) self.joystick_alt = wpilib.Joystick(2) # Buttons self.btn_launcher_solenoid = JoystickButton(self.joystick_alt, 1) self.btn_intake_ = JoystickButton(self.joystick_alt, 5) self.btn_align = JoystickButton(self.joystick_left, 1) self.btn_intake_in = JoystickButton(self.joystick_alt, 3) self.btn_intake_out = JoystickButton(self.joystick_alt, 4) self.btn_cp_extend = Toggle(self.joystick_left, 4) self.btn_winch = JoystickButton(self.joystick_alt, 8) self.btn_cp_motor = JoystickButton(self.joystick_left, 3) self.btn_launcher_motor = JoystickButton(self.joystick_alt, 12) self.btn_launcher_idle = Toggle(self.joystick_alt, 10) self.btn_launcher_motor_close = JoystickButton(self.joystick_alt, 11) # Initiation Line self.btn_launcher_motor_dynamic = JoystickButton(self.joystick_alt, 9) self.btn_slow_movement = JoystickButton(self.joystick_right, 1) self.btn_intake_solenoid = Toggle(self.joystick_alt, 2) self.btn_scissor_extend = Toggle(self.joystick_alt, 7) self.btn_color_sensor = JoystickButton(self.joystick_left, 5) self.btn_cp_stop = JoystickButton(self.joystick_left, 2) self.btn_invert_y_axis = JoystickButton(self.joystick_left, 6) self.btn_rotation_sensitivity = JoystickButton(self.joystick_right, 1) self.btn_intake_bottom_out = JoystickButton(self.joystick_alt, 6) # Set up motors for encoders self.master_left = CANSparkMax(1, MotorType.kBrushless) self.master_right = CANSparkMax(4, MotorType.kBrushless) self.encoder_constant = (1 / self.GEARING) * self.WHEEL_DIAMETER * math.pi self.left_encoder = self.master_left.getEncoder() self.left_encoder.setPositionConversionFactor(self.encoder_constant) self.left_encoder.setVelocityConversionFactor(self.encoder_constant / 60) self.right_encoder = self.master_right.getEncoder() self.right_encoder.setPositionConversionFactor(self.encoder_constant) self.right_encoder.setVelocityConversionFactor(self.encoder_constant / 60) self.left_encoder.setPosition(0) self.right_encoder.setPosition(0) # Set up Speed Controller Groups self.left_motors = wpilib.SpeedControllerGroup( self.master_left, CANSparkMax(3, MotorType.kBrushless)) self.right_motors = wpilib.SpeedControllerGroup( self.master_right, CANSparkMax(6, MotorType.kBrushless)) # Drivetrain self.train = wpilib.drive.DifferentialDrive(self.left_motors, self.right_motors) # Winch self.winch_motors = wpilib.SpeedControllerGroup( CANSparkMax(8, MotorType.kBrushless), CANSparkMax(9, MotorType.kBrushless)) self.scissor_solenoid = wpilib.DoubleSolenoid(6, 7) self.hook_motor = WPI_VictorSPX(3) # Control Panel Spinner self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard) self.cp_solenoid = wpilib.DoubleSolenoid( 5, 4) # Reversed numbers so kForward is extended self.cp_motor = WPI_VictorSPX(2) self.ultrasonic = Ultrasonic(3, 4) self.ultrasonic.setAutomaticMode(True) self.ultrasonic.setEnabled(True) # Intake self.intake_motor = WPI_VictorSPX(1) self.intake_motor_lower = CANSparkMax(40, MotorType.kBrushed) self.intake_solenoid = wpilib.DoubleSolenoid(2, 1) self.intake_switch = wpilib.DigitalInput(2) # Launcher # self.launcher_spark = CANSparkMax(40, MotorType.kBrushed) # self.launcher_spark.setInverted(True) self.launcher_motor = CANSparkMax(10, MotorType.kBrushed) self.launcher_motor.setInverted(True) self.launcher_motor_follower = CANSparkMax(12, MotorType.kBrushed) self.launcher_motor_follower.follow(self.launcher_motor) self.launcher_solenoid = wpilib.Solenoid(0) # Don't use index pin and change to k1X encoding to decrease rate measurement jitter self.launcher_encoder = self.launcher_motor.getEncoder( CANEncoder.EncoderType.kQuadrature, 8192) self.rpm_controller = self.launcher_motor.getPIDController() self.launcher_sensor = wpilib.Ultrasonic(0, 1) self.launcher_sensor.setAutomaticMode(True) self.launcher_sensor.setEnabled(True) self.launcher_encoder.setPosition(0) # NavX (purple board on top of the RoboRIO) self.navx = navx.AHRS.create_spi() self.navx.reset() # Limelight self.limelight = Limelight() # Camera Stream CameraServer.launch('camera/camera.py:main') # Robot motion control self.kinematics = KINEMATICS # Use kinematics from inner trajectory generator code self.drive_feedforward = DRIVE_FEEDFORWARD self.trajectories = load_trajectories() # LED strip self.led_driver = BlinkinLED(0) def autonomous(self): self.right_motors.setInverted(True) super().autonomous() self.limelight.changePipeline(0) def disabledInit(self): self.navx.reset() self.limelight_stream_mode = 0 def disabledPeriodic(self): self.limelight.averagePose() def teleopInit(self): self.right_motors.setInverted(False) self.drive.squared_inputs = True self.drive.speed_constant = 1.05 self.limelight.changePipeline(0) self.drive.rotational_constant = 0.5 self.inverse = 1 def teleopPeriodic(self): # if self.btn_invert_y_axis.get(): # self.flipped = True # self.inverse *= -1 # else: # self.flipped = False # self.logger.info(self.ultrasonic.getRangeInches()) if self.btn_invert_y_axis.get(): self.inverse = 1 else: self.inverse = -1 if self.btn_rotation_sensitivity.get(): self.drive.rotational_constant = 0.1 self.drive.move(self.inverse * self.joystick_left.getY(), self.joystick_right.getX()) # Align (Overrides self.drive.move() because it's placed after) if self.btn_align.get() and self.limelight.targetExists(): self.drive.set_target(self.limelight.getYaw(), relative=True) if self.btn_align.get(): self.limelight.TurnLightOn(True) self.drive.align() else: self.limelight.TurnLightOn(False) self.drive.set_target(None) if self.btn_slow_movement.get(): # 10% of original values self.drive.rotational_constant = 0.54 self.drive.speed_constant = 0.5 # Control Panel Spinner self.control_panel.set_solenoid(self.btn_cp_extend.get()) if self.btn_cp_extend.get(): self.ntSolenoid_state = True else: self.ntSolenoid_state = False if self.btn_scissor_extend.get(): self.scissor_solenoid.set(wpilib.DoubleSolenoid.Value.kForward) else: self.scissor_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse) # Color Sensor if self.btn_color_sensor.get(): self.panel_spinner.spin_to() if self.btn_cp_motor.get(): self.control_panel.spin(0.5) # Launcher if self.btn_launcher_motor.get(): self.launcher.setVelocity(4630) elif self.btn_launcher_motor_close.get(): self.launcher.setVelocity(3900) elif self.btn_launcher_motor_dynamic.get(): # self.limelight.TurnLightOn(True) self.launcher.setVelocity(self.ds_velocity_setpoint) # if self.limelight.targetExists(): # self.launcher.setVelocityFromDistance(self.limelight.pitch_angle, 4670) elif self.btn_launcher_idle.get(): self.launcher.setPercentOutput(1) if self.btn_launcher_solenoid.get(): self.auto_launcher.fire_when_ready() if self.btn_cp_stop.get(): self.panel_spinner.done() # Intake if self.btn_intake_out.get(): self.intake.spin_inside(1) elif self.btn_intake_in.get(): self.intake.spin(-1, 0.4) elif self.btn_intake_bottom_out.get(): self.intake.spin_lower(-0.4) if self.btn_intake_solenoid.get(): self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse) else: self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kForward) # Winch if self.btn_winch.get(): self.winch_motors.set(0.65) else: self.winch_motors.set( 0 ) # Must use set(0) when not pressed because there is no component # Hook if self.joystick_alt.getPOV(0) == 90: self.hook_motor.set(0.5) elif self.joystick_alt.getPOV(0) == 270: self.hook_motor.set(-0.5) else: self.hook_motor.set(0) if self.joystick_alt.getPOV(0) == 0: self.launcher.fire()
class VictisVision: STREAM_CV = False secondary_cam = ntproperty('/camera/control/secondary_cam', False) cv_enabled = ntproperty('/camera/control/cv_enabled', False) dark_exposure = ntproperty('/camera/control/dark_exposure', 3) test = ntproperty('/camera/control/test', 0) def __init__(self): self.nt = NetworkTable.getTable('/camera') #Cameras self.piston_cam = cs.UsbCamera('Piston Cam', 0) self.piston_cam.setVideoMode(cs.VideoMode.PixelFormat.kMJPEG, 160, 120, 35) #160 vs. 120 self.piston_cam.setExposureAuto() self.piston_cam.getProperty('backlight_compensation').set(5) #self.piston_cam.setExposureManual(35) #self.piston_cam.setBrightness(65) #test = self.piston_cam.getProperty("exposure_absolute") #print(self.piston_cam.getProperty("exposure_absolute")) self.piston_server = cs.MjpegServer('httpserver', 1181) self.piston_server.setSource(self.piston_cam) if self.secondary_cam: self.light_ring_cam = cs.UsbCamera('Light Ring Cam', 0) self.light_ring_cam.setVideoMode(cs.VideoMode.PixelFormat.kMJPEG, 320, 240, 20) # This only seems to affect automatic exposure mode # -> higher value means it lets more light in when facing a big light self.light_ring_cam.getProperty('backlight_compensation').set(5) #Image Processing self.cvsink = cs.CvSink('cvsink') self.cvsink.setSource(self.light_ring_cam) self.cvsource = cs.CvSource('cvsource', cs.VideoMode.PixelFormat.kMJPEG, 320, 240, 20) #Streaming Servers #self.ring_stream = cs.MjpegServer("ring server", 1182) #self.ring_stream.setSource(self.light_ring_cam) if self.STREAM_CV: self.cv_stream = cs.MjpegServer('cv stream', 1183) self.cv_stream.setSource(self.cvsource) #Blank mat self.img = np.zeros(shape=(320, 240, 3), dtype=np.uint8) self.processor = ImageProcessor() def process(self): exposure = None while True: if self.secondary_cam: if self.cv_enabled: if exposure != 'dark': self.light_ring_cam.setExposureManual( int(self.dark_exposure)) exposure = 'dark' time, self.img = self.cvsink.grabFrame(self.img) if time == 0: self.cvsource.notifyError(self.cvsink.getError()) continue self.img = self.processor.process_frame(self.img, time) else: if exposure != 'auto': self.light_ring_cam.setExposureAuto() exposure = 'auto' self.nt.putBoolean('processor/gear_target_present', False) if self.STREAM_CV: self.cvsource.putFrame(self.img)
class DataLogger: # Change this key to whatever NT key you want to log log_key = '/SmartDashboard/log_data' # Data file where robot IP is stored so you don't have to keep typing it cache_file = '.robot' matchNumber = ntproperty('/FMSInfo/MatchNumber', 0, False) eventName = ntproperty('/FMSInfo/EventName', 'unknown', False) def __init__(self): self.queue = queue.Queue() self.mode = 'disabled' self.data = [] self.lock = threading.Lock() def connectionListener(self, connected, info): # set our robot to 'disabled' if the connection drops so that we can # guarantee the data gets written to disk if not connected: self.valueChanged('/FMSInfo/FMSControlData', 0, False) def valueChanged(self, key, value, isNew): if key == '/FMSInfo/FMSControlData': mode = translate_control_word(value) with self.lock: last = self.mode self.mode = mode data = self.data self.data = [] logger.info("Robot mode: %s -> %s", last, mode) # This example only stores on auto -> disabled transition. Change it # to whatever it is that you need for logging if last == 'auto': tm = time.strftime('%Y%m%d-%H%M-%S') name = '%s-%s-%s.json' % (tm, self.eventName, int(self.matchNumber)) logger.info('New file: %s (%d items received)', name, len(data)) # We don't write the file from within the NetworkTables callback, # because we don't want to block the thread. Instead, write it # to a queue along with the filename so it can be written # from somewhere else self.queue.put((name, data)) elif key == self.log_key: if self.mode != 'disabled': with self.lock: self.data.append(value) def run(self): # Determine what IP to connect to try: server = sys.argv[1] # Save the robot ip if not os.path.exists(self.cache_file): with open(self.cache_file, 'w') as fp: fp.write(server) except IndexError: try: with open(self.cache_file) as fp: server = fp.read().strip() except IOError: print("Usage: logger.py [10.xx.yy.2]") return logger.info("NT server set to %s", server) NetworkTables.initialize(server=server) # Use listeners to receive the data NetworkTables.addConnectionListener(self.connectionListener, immediateNotify=True) NetworkTables.addEntryListener(self.valueChanged) # When new data is queued, write it to disk while True: name, data = self.queue.get() with open(name, 'w') as fp: json.dump(data, fp)
class MyRobot(wpilib.TimedRobot): """Main robot class""" # NetworkTables API for controlling this #: Speed to set the motors to autospeed = ntproperty("/robot/autospeed", defaultValue=0, writeDefault=True) #: Test data that the robot sends back telemetry = ntproperty("/robot/telemetry", defaultValue=(0, ) * 9, writeDefault=False) prior_autospeed = 0 WHEEL_DIAMETER = 0.5 ENCODER_PULSE_PER_REV = 4096 PIDIDX = 0 def robotInit(self): """Robot-wide initialization code should go here""" self.lstick = wpilib.Joystick(0) # Left front left_front_motor = ctre.WPI_TalonSRX(1) left_front_motor.setInverted(False) left_front_motor.setSensorPhase(False) self.left_front_motor = left_front_motor # Right front right_front_motor = ctre.WPI_TalonSRX(2) right_front_motor.setInverted(False) right_front_motor.setSensorPhase(False) self.right_front_motor = right_front_motor # Left rear -- follows front left_rear_motor = ctre.WPI_TalonSRX(3) left_rear_motor.setInverted(False) left_rear_motor.setSensorPhase(False) left_rear_motor.follow(left_front_motor) # Right rear -- follows front right_rear_motor = ctre.WPI_TalonSRX(4) right_rear_motor.setInverted(False) right_rear_motor.setSensorPhase(False) right_rear_motor.follow(right_front_motor) # # Configure drivetrain movement # self.drive = DifferentialDrive(left_front_motor, right_front_motor) self.drive.setDeadband(0) # # Configure encoder related functions -- getpos and getrate should return # ft and ft/s # encoder_constant = ((1 / self.ENCODER_PULSE_PER_REV) * self.WHEEL_DIAMETER * math.pi) left_front_motor.configSelectedFeedbackSensor( left_front_motor.FeedbackDevice.QuadEncoder, self.PIDIDX, 10) self.l_encoder_getpos = ( lambda: left_front_motor.getSelectedSensorPosition( self.PIDIDX) * encoder_constant) self.l_encoder_getrate = ( lambda: left_front_motor.getSelectedSensorVelocity( self.PIDIDX) * encoder_constant * 10) right_front_motor.configSelectedFeedbackSensor( right_front_motor.FeedbackDevice.QuadEncoder, self.PIDIDX, 10) self.r_encoder_getpos = ( lambda: left_front_motor.getSelectedSensorPosition( self.PIDIDX) * encoder_constant) self.r_encoder_getrate = ( lambda: left_front_motor.getSelectedSensorVelocity( self.PIDIDX) * encoder_constant * 10) # # Configure gyro # # You only need to uncomment the below lines if you want to characterize wheelbase radius # Otherwise you can leave this area as-is self.gyro_getangle = lambda: 0 # Uncomment for the KOP gyro # Note that the angle from all implementors of the Gyro class must be negated because # getAngle returns a clockwise angle, and we require a counter-clockwise one # gyro = ADXRS450_Gyro() # self.gyro_getangle = lambda: -1 * gyro.getAngle() # Set the update rate instead of using flush because of a NetworkTables bug # that affects ntcore and pynetworktables # -> probably don't want to do this on a robot in competition NetworkTables.setUpdateRate(0.010) def disabledInit(self): self.logger.info("Robot disabled") self.drive.tankDrive(0, 0) def disabledPeriodic(self): pass def robotPeriodic(self): # feedback for users, but not used by the control program sd = wpilib.SmartDashboard sd.putNumber("l_encoder_pos", self.l_encoder_getpos()) sd.putNumber("l_encoder_rate", self.l_encoder_getrate()) sd.putNumber("r_encoder_pos", self.r_encoder_getpos()) sd.putNumber("r_encoder_rate", self.r_encoder_getrate()) sd.putNumber("gyro_angle", self.gyro_getangle()) def teleopInit(self): self.logger.info("Robot in operator control mode") def teleopPeriodic(self): self.drive.arcadeDrive(-self.lstick.getY(), self.lstick.getX()) def autonomousInit(self): self.logger.info("Robot in autonomous mode") def autonomousPeriodic(self): """ If you wish to just use your own robot program to use with the data logging program, you only need to copy/paste the logic below into your code and ensure it gets called periodically in autonomous mode Additionally, you need to set NetworkTables update rate to 10ms using the setUpdateRate call. Note that reading/writing self.autospeed and self.telemetry are NetworkTables operations (using pynetworktables's ntproperty), so if you don't read/write NetworkTables in your implementation it won't actually work. """ # Retrieve values to send back before telling the motors to do something now = wpilib.Timer.getFPGATimestamp() l_encoder_position = self.l_encoder_getpos() l_encoder_rate = self.l_encoder_getrate() r_encoder_position = self.r_encoder_getpos() r_encoder_rate = self.r_encoder_getrate() battery = self.ds.getBatteryVoltage() l_motor_volts = self.left_front_motor.getMotorOutputVoltage() r_motor_volts = self.right_front_motor.getMotorOutputVoltage() gyro_angle = self.gyro_getangle() # Retrieve the commanded speed from NetworkTables autospeed = self.autospeed self.prior_autospeed = autospeed # command motors to do things self.drive.tankDrive(autospeed, autospeed, False) # send telemetry data array back to NT self.telemetry = ( now, battery, autospeed, l_motor_volts, r_motor_volts, l_encoder_position, r_encoder_position, l_encoder_rate, r_encoder_rate, gyro_angle, )
class Finn(TimedRobot): elevator_distance = ntproperty("/SmartDashboard/elevator_distance", defaultValue=0.5, writeDefault=True) chassis = Chassis(ID.Drive.Left.master, ID.Drive.Right.master, left_slave_ids=ID.Drive.Left.slaves, right_slave_ids=ID.Drive.Right.slaves, low_gear_solenoid_port=ID.Drive.LowGear.channel, high_gear_solenoid_port=ID.Drive.HighGear.channel) intake = Intake(ID.Intake.front, ID.Intake.back) elevator = Elevator(ID.Elevator.master, ID.Elevator.slaves) hatch_floor_grabber = HatchFloorGrabber(ID.HatchFloorGrabber.rotate, ID.HatchFloorGrabber.intake) hatch_holder = HatchHolder(ID.HatchHolder.extend_port, ID.HatchHolder.retract_port, ID.HatchHolder.open_port, ID.HatchHolder.close_port) cargo_grabber = CargoGrabber(ID.CargoGrabber.rotate, ID.CargoGrabber.intake) subsystems = [ chassis, intake, elevator, hatch_floor_grabber, hatch_holder, cargo_grabber ] dashboard = NetworkTables.getTable("SmartDashboard") def robotInit(self): print("hello world") self.joystick = XBox(0) self.path = None self.chassis.reset_sensors() self.elevator.reset_sensors() def autonomousInit(self): try: self.chassis.reset_sensors() self.elevator.reset_sensors() self.elevator.low() self.teleopInit() except Exception as e: print(e) def autonomousPeriodic(self): self.teleopPeriodic() def teleopPeriodic(self): # chassis self.chassis.update() self.chassis.arcade_drive(self.joystick.get_drive_forward(), self.joystick.get_drive_turn() * 0.7) if self.joystick.get_shift_gears(): self.chassis.shift() # hatch floor grabber # self.hatch_floor_grabber.update() # self.hatch_floor_grabber.hold_up() # hatch holder self.hatch_holder.update() if self.joystick.get_intake_hatch(): print("intaking") self.hatch_holder.intake() elif self.joystick.get_drop_hatch(): print("dropping") self.hatch_holder.drop() elif self.joystick.get_grab_hatch(): print("grabbing") self.hatch_holder.grab() elif self.hatch_holder.is_intaking(): print("holding") self.hatch_holder.hold() # cargo grabber self.cargo_grabber.update() if self.joystick.get_rotate_cargo_down(): print("getting cargo") self.cargo_grabber.get_cargo() self.elevator.zero() elif self.joystick.get_rotate_cargo_up(): self.cargo_grabber.stop_getting_cargo() # intake self.intake.update() if self.joystick.get_shoot(): self.intake.shoot() elif not self.cargo_grabber.is_stopping(): self.intake.slow_intake() else: self.intake.idle() # elevator self.elevator.update() if self.joystick.get_elevator_low(): self.elevator.low() elif self.joystick.get_elevator_mid(): self.elevator.mid() elif self.joystick.get_elevator_high(): self.elevator.high() elif self.joystick.get_elevator_cargo_ship(): self.elevator.cargo_ship()
class TargetGoal(StateMachine): # Aliases intake = intake.Arm drive = drive.Drive shootBall = shootBall.ShootBall # Fetch NetworkTables values present = ntproperty('/components/autoaim/present', False) targetHeight = ntproperty('/components/autoaim/target_height', 0) idealHeight = tunable(-7) heightThreshold = tunable(-7) shoot = False def target(self): """When called, seek target""" self.engage() self.shoot = False def target_shoot(self): """When called, seek target then shoot""" self.engage() self.shoot = True @state(first=True) def align(self, initial_call): """First state: turn robot to be facing tower""" # If it's the first time if initial_call: # Then turn on vision system self.drive.enable_camera_tracking() # If it's inline with the tower and it's been told to shoot when done aiming, if self.drive.align_to_tower() and self.shoot: # Then move on to shooting state. self.next_state('camera_assisted_drive') @state def camera_assisted_drive(self): """Second state: go forward to shooting location""" # If target is close if self.targetHeight > self.heightThreshold: # Move forward, guided by threshold self.drive.move( max(abs(self.idealHeight - self.targetHeight) / 55, .5), 0) # Align to tower. Pretty self explanitory. self.drive.align_to_tower() else: # Otherwise, move on to next step, shooting. self.next_state('shoot') @state def shoot(self): """Align to tower and fire ball.""" self.drive.align_to_tower() self.shootBall.shoot() def done(self): """Finish up, disable everything.""" # Kill camera tracking self.drive.disable_camera_tracking() # Stop state machine StateMachine.done(self)
class BallsFirst(AutonomousStateMachine): MODE_NAME = 'BallsFirst' """ Back up to pick up two balls, then drive back and shoot all five balls. """ starting_pos = ntproperty('/autonomous/starting_position', StartingPosition.LEFT.name) drive: Drive follower: Follower odometry: Odometry intake: Intake limelight: Limelight right_motors: wpilib.SpeedControllerGroup launcher: Launcher def on_enable(self): super().on_enable() self.shot_count = 0 start_pos = self.starting_pos # if start_pos == 'LIMELIGHT': # self.odometry.reset(self.limelight.getAveragedPose()) # else: self.odometry.reset(TRAJECTORIES['trench-ball-2'].start) @follower_state(trajectory_name='trench-ball-2', next_state='trench_return', first=True) def move_trench(self, tm, state_tm, initial_call): self.intake.spin(-0.53) @follower_state(trajectory_name='trench-ball-2-return', next_state='align') def trench_return(self, tm, state_tm, initial_call): self.launcher.setVelocity(2000) self.limelight.TurnLightOn(True) @state def align(self): self.launcher.setVelocity(2000) self.limelight.TurnLightOn(True) if self.limelight.targetExists(): self.drive.set_target(self.limelight.getYaw() + 2, relative=True) if self.drive.angle_setpoint is not None: self.drive.align() if self.drive.target_locked: self.next_state('spinup') @state def spinup(self, state_tm, initial_call): self.limelight.TurnLightOn(True) if self.shot_count >= 9: self.next_state('realign') if self.shot_count >= 3: self.intake.spin(-0.93) # Wait until shooter motor is ready self.launcher.setVelocity(4440) if self.shot_count >= 3 and state_tm < 0.5: return if self.launcher.at_setpoint() and (self.launcher.ball_found() or self.shot_count >= 4) and not initial_call: self.next_state('shoot') @timed_state(duration=0.75, next_state='spinup') def shoot(self, state_tm, initial_call): self.limelight.TurnLightOn(True) if initial_call: self.shot_count += 1 if self.shot_count >= 3: self.intake.spin(-0.93) self.launcher.setVelocity(4440) if state_tm < 0.25: self.launcher.fire() @state def realign(self): self.limelight.TurnLightOn(False) self.drive.calculated_pid = False self.drive.set_target(0, relative=False) self.drive.align() if self.drive.target_locked: self.done()
class Foo(object): robotTime = ntproperty('/SmartDashboard/robotTime', 0, writeDefault=False) dsTime = ntproperty('/SmartDashboard/dsTime', 0, writeDefault=True)
class Foo2(object): testArray = ntproperty('/SmartDashboard/testArray', [], writeDefault=False)
class AutoAlign(StateMachine): drive = SwerveDrive x_ctrl = XPosController y_ctrl = YPosController angle_ctrl = AngleController pos_history = PositionHistory cv_enabled = ntproperty('/camera/control/cv_enabled', False) ideal_skew = tunable(-0.967) ideal_angle = tunable(-1.804) def __init__(self): target = None nt = NetworkTable.getTable('/camera') nt.addTableListener(self._on_target, True, 'target') self.aimed_at_angle = None self.aimed_at_x = None def _on_target(self, source, key, value, isNew): self.target = value def _move_to_position(self): target = self.target if target is not None and len(target) > 0: target = target[:] angle, skew, capture_ts = target history = self.pos_history.get_position(capture_ts) if history is not None: r_angle, r_x, r_y, r_time = history self.aimed_at_angle = r_angle + angle - self.ideal_angle self.target = None if self.aimed_at_angle is not None: self.angle_ctrl.align_to(self.aimed_at_angle) return self.angle_ctrl.is_aligned() def align(self): self.engage() @state(first=True) def inital_state(self): self.target = None self.aimed_at_angle = None self.pos_history.enable() self.cv_enabled = True self.next_state('moving_to_position') @state def moving_to_position(self): if self._move_to_position(): self.next_state('done') def done(self): super().done() self.pos_history.disable() self.cv_enabled = False
class MyRobot(magicbot.MagicRobot): # Shorten a bunch of things targetGoal = targetGoal.TargetGoal shootBall = shootBall.ShootBall winch = winch.Winch light = light.Light lightSwitch = lightOff.LightSwitch intake = intake.Arm drive = drive.Drive enable_camera_logging = ntproperty('/camera/logging_enabled', True) auto_aim_button = ntproperty('/SmartDashboard/Drive/autoAim', False, writeDefault = False) """Create basic components (motor controllers, joysticks, etc.)""" def createObjects(self): # Joysticks self.joystick1 = wpilib.Joystick(0) self.joystick2 = wpilib.Joystick(1) # Motors (l/r = left/right, f/r = front/rear) self.lf_motor = wpilib.CANTalon(5) self.lr_motor = wpilib.CANTalon(10) self.rf_motor = wpilib.CANTalon(15) self.rr_motor = wpilib.CANTalon(20) # Drivetrain object self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor, self.rf_motor, self.rr_motor) # Left and right arm motors (there's two, which both control the raising and lowering the arm) self.leftArm = wpilib.CANTalon(25) self.rightArm = wpilib.CANTalon(30) # Motor that spins the bar at the end of the arm. # There was originally going to be one on the right, but we decided against that in the end. # In retrospect, that was probably a mistake. self.leftBall = wpilib.Talon(9) # Motor that reels in the winch to lift the robot. self.winchMotor = wpilib.Talon(0) # Motor that opens the winch. self.kickMotor = wpilib.Talon(1) # Aiming flashlight self.flashlight = wpilib.Relay(0) # Timer to keep light from staying on for too long self.lightTimer = wpilib.Timer() # Flashlight has three intensities. So, when it's turning off, it has to go off on, off on, off. # self.turningOffState keeps track of which on/off it's on. self.turningOffState = 0 # Is currently on or off? Used to detect if UI button is pressed. self.lastState = False # Drive encoders; measure how much the motor has spun self.rf_encoder = driveEncoders.DriveEncoders(self.robot_drive.frontRightMotor, True) self.lf_encoder = driveEncoders.DriveEncoders(self.robot_drive.frontLeftMotor) # Distance sensors self.back_sensor = distance_sensors.SharpIRGP2Y0A41SK0F(0) self.ultrasonic = wpilib.AnalogInput(1) # NavX (purple board on top of the RoboRIO) self.navX = navx.AHRS.create_spi() # Initialize SmartDashboard, the table of robot values self.sd = NetworkTable.getTable('SmartDashboard') # How much will the control loop pause in between (0.025s = 25ms) self.control_loop_wait_time = 0.025 # Button to reverse controls self.reverseButton = ButtonDebouncer(self.joystick1, 1) # Initiate functional buttons on joysticks self.shoot = ButtonDebouncer(self.joystick2, 1) self.raiseButton = ButtonDebouncer(self.joystick2, 3) self.lowerButton = ButtonDebouncer(self.joystick2, 2) self.lightButton = ButtonDebouncer(self.joystick1, 6) def autonomous(self): """Prepare for autonomous mode""" # Reset Gyro to 0 self.drive.reset_gyro_angle() # Call autonomous magicbot.MagicRobot.autonomous(self) def disabledPeriodic(self): """Repeat periodically while robot is disabled. Usually emptied. Sometimes used to easily test sensors and other things.""" pass def disabledInit(self): """Do once right away when robot is disabled.""" self.enable_camera_logging = True self.drive.disable_camera_tracking() def teleopInit(self): """Do when teleoperated mode is started.""" self.drive.reset_drive_encoders() self.sd.putValue('startTheTimer', True) self.intake.target_position = None self.intake.target_index = None self.drive.disable_camera_tracking() self.enable_camera_logging = False def teleopPeriodic(self): """Do periodically while robot is in teleoperated mode.""" # Get the joystick values and move as much as they say. self.drive.move(-self.joystick1.getY(), self.joystick2.getX()) # If reverse control button is pressed, if self.reverseButton.get(): # Reverse the drivetrain direction self.drive.switch_direction() # If outtake button is pressed, if self.joystick2.getRawButton(5): # Then spit ball out. self.intake.outtake() # Or, if intake button is pressed, elif self.joystick2.getRawButton(4): # Then suck button in. self.intake.intake() # If shoot button pressed if self.shoot.get(): # Automatically shoot ball self.shootBall.shoot() """There's two sets of arm buttons. The first automatically raises and lowers the arm the proper amount, whereas the second will let you manually raise and lower it more precise amounts.""" # If automatic arm raise button is pressed, if self.raiseButton.get(): # Raise arm self.intake.raise_arm() # Or, if automatic arm lower button is pressed, (won't do both at once) elif self.lowerButton.get(): # Lower arm self.intake.lower_arm() # If manual arm raise button is pressed, if self.joystick1.getRawButton(3): # Raise arm self.intake.set_manual(-1) # If manual arm lower button is pressed, (this one can be activated both at one time) if self.joystick1.getRawButton(2): # Lower arm self.intake.set_manual(1) # Flashlight # Automatically turn flashlight off at the starting. It will only be made true if NT value is true. lightButton = False # Store whether flashlight button is pressed on dashboard uiButton = self.sd.getValue('LightBulb', False) # If the value has changed if uiButton != self.lastState: # Flashlight on lightButton = True # Update self.lastState to the new state self.lastState = uiButton # If light button on joystick or light button is pressed and turn-off state is 0 if (self.lightButton.get() or lightButton) and self.turningOffState == 0: # Turn on flashlight self.lightSwitch.switch() # If joystick button 5 or UI autoaim button is pressed if self.joystick1.getRawButton(5) or self.auto_aim_button: # Start targeting goal self.targetGoal.target() # Winch # If joystick1 button 7 is pressed if self.joystick1.getRawButton(7): # Set off winch self.winch.deploy_winch() # If joystick1 button 8 is pressed if self.joystick1.getRawButton(8): # Reel in winch self.winch.winch() # If button 9 on joystick1 pressed and robot is backwards if self.joystick1.getRawButton(9) and self.drive.isTheRobotBackwards: # Move self.drive.move(.5, 0) # Testing angles in pit or when not in competition # If Field Management System isn't attached if not self.ds.isFMSAttached(): # If button 10 on joystick1 is pressed if self.joystick1.getRawButton(10): # Calibrate rotation angle to 35deg self.drive.angle_rotation(35) # If button 9 on joystick1 is pressed elif self.joystick1.getRawButton(9): # Could be problematic if robot is backwards # Calibrate robot rotation angle to 0 self.drive.angle_rotation(0) # If button 10 on joystick2 is pressed elif self.joystick2.getRawButton(10): # Activate vision things self.drive.enable_camera_tracking() self.drive.align_to_tower()
class MyRobot(wpilib.TimedRobot): TIMEOUT_MS = 30 velocity_p = .1 #ntproperty('/encoders/velocity_p', .1, persistent = True) velocity_i = .0001 #ntproperty('/encoders/velocity_i', 0.0001, persistent = True) velocity_d = 0 #ntproperty('/encoders/velocity_d', 0.0, persistent = True) velocity_f = .1 #ntproperty('/encoders/velocity_f', .1, persistent=True) position_p = ntproperty('/encoders/position_p', .5, persistent=True) position_i = ntproperty('/encoders/position_i', 0.0, persistent=True) position_d = ntproperty('/encoders/position_d', 0.0, persistent=True) position_f = ntproperty('/encoders/position_f', .7, persistent=True) elevator_p = ntproperty('/encoders/elevator_p', 0.0, persistent=True) elevator_i = ntproperty('/encoders/elevator_i', 0.0, persistent=True) elevator_d = ntproperty('/encoders/elevator_d', 0.0, persistent=True) elevator_f = ntproperty('/encoders/elevator_f', 0.0, persistent=True) back_lift_speed_up = ntproperty('/lifts/back_lift_speed_up', .5, persistent=True) back_lift_speed_down = ntproperty('/lifts/back_lift_speed_down', .7, persistent=True) front_lift_speed_up = ntproperty('/lifts/front_lift_speed_up', 1, persistent=True) front_lift_speed_down = ntproperty('/lifts/front_lift_speed_down', .3, persistent=True) arm_speed_up = .5 #ntproperty('/lifts/arm_speed_up', 1, persistent=True) arm_speed_down = .1 #ntproperty('/lifts/arm_speed_down', .3, persistent=True)/2 front_raised_max = ntproperty('/lifts/max', 31500, persistent=True) front_bottom = ntproperty('/lifts/min', -1000, persistent=True) lift_adjust_value = ntproperty('/lifts/adjust_value', 1000, persistent=True) ntproperty('/lifts/.type', 'Adjustable') arm_adjust_value = ntproperty('/arms/adjust_value', .1, persistent=True) open_state = ntproperty('/arms/max', .02, persistent=True) closed_state = ntproperty('/arms/min', .35, persistent=True) ntproperty('/arms/.type', 'Adjustable') lift_divider = ntproperty('/lifts/lift_divider', 3, persistent=True) lift_speed_up = ntproperty('/lifts/lift_speed_up', 1, persistent=True) lift_speed_down = ntproperty('/lifts/lift_speed_down', .3, persistent=True) talon_ramp = ntproperty('/encoders/talon_ramp', 0, persistent=True) continuous_current_limit = ntproperty('/encoder/continuous_current_limit', 0, persistent=True) peak_current_limit = ntproperty('/encoder/peak_current_limit', 0, persistent=True) lift_limits = ntproperty('/encoders/lift_limits', False, persistent=True) displacement_multiplier = ntproperty("/encoders/displacement_multiplier", 1, persistent=True) velocity_mode = ntproperty('/encoders/velocity_mode', True) servo_position = ntproperty('/Servo/Value', .5, persistent=True) servo_offset1 = ntproperty('/Servo/Offset1', 0, persistent=True) servo_offset2 = ntproperty('/Servo/Offset2', 0, persistent=True) arm_up = ntproperty('/Servo/ArmUp', 0, persistent=True) arm_down = ntproperty('/Servo/ArmDown', 0, persistent=True) #liftforball = ntproperty('/Servo/ArmforBall', 0.5, persistent = True) ticks_per_rev = ntproperty('/encoders/ticks_per_rev', 1440, persistent=True) wheel_diameter = ntproperty('/encoders/wheel_diameter', 6, persistent=True) max_speed = ntproperty('/encoders/max_speed', 10, persistent=True) wheel_diameter = ntproperty('/encoders/wheel_diameter', 6, persistent=True) field_centric = ntproperty('/encoders/field_centric', False, persistent=True) ticks_per_rev_fl = 12000 #ntproperty('/encoders/ticks_per_rev_fl', 8630, persistent = True) # done ticks_per_rev_bl = 12000 #ntproperty('/encoders/ticks_per_rev_bl', 8630, persistent = True) # done ticks_per_rev_fr = 12000 #ntproperty('/encoders/ticks_per_rev_fr', 8630, persistent = True) # done ticks_per_rev_br = 12000 #ntproperty('/encoders/ticks_per_rev_br', 8630, persistent = True) # done def setMotorPids(self, motor, p, i, d, f): #print("setting encoder PIDs") motor.config_kP(0, p, MyRobot.TIMEOUT_MS) motor.config_kI(0, i, MyRobot.TIMEOUT_MS) motor.config_kD(0, d, MyRobot.TIMEOUT_MS) motor.config_kF(0, f, MyRobot.TIMEOUT_MS) turn_rate_p = ntproperty('/gyro/turn_rate_p', 0, persistent=True) turn_rate_i = ntproperty('/gyro/turn_rate_i', 0, persistent=True) turn_rate_d = ntproperty('/gyro/turn_rate_d', 0, persistent=True) turn_rate_pid_input_range = ntproperty('/gyro/pid_input_range', 180, persistent=True) turn_rate_pid_output_range = ntproperty('/gyro/pid_output_range', 1, persistent=True) pause_time = ntproperty('/gyro/pause_time', 1, persistent=True) max_turn_rate = ntproperty("/gyro/max_turn_rate", 120, persistent=True) lift_target = ntproperty("/lifts/lift_target", 0) front_lift_heights = [ 0, 3000, 6800, 6800, 10700, 15792, 18529, 21500, 29500, 31500 ] #ntproperty("/lifts/front_lift_heights", [1,2,3,4,5,6], persistent=True) front_lift_heights_index = ntproperty("/lifts/front_lift_heights_index", 0, persistent=True) climb_toggle = ntproperty('/lifts/climb_toggle', False, persistent=True) match_time = ntproperty('/time/match-time', 0.0) def front_lift_increment(self): if self.front_lift_heights_index < (len(self.front_lift_heights) - 1): self.front_lift_heights_index += 1 def front_lift_decrement(self): if self.front_lift_heights_index > 0: self.front_lift_heights_index -= 1 def robotInit(self): self.BUTTON_RBUMPER = 6 self.BUTTON_LBUMPER = 5 self.BUTTON_A = 1 self.BUTTON_B = 2 self.BUTTON_X = 3 self.BUTTON_Y = 4 self.LY_AXIS = 1 self.LX_AXIS = 0 self.RX_AXIS = 4 self.RY_AXIS = 5 self.R_TRIGGER = 3 self.L_TRIGGER = 2 self.LEFT_JOYSTICK_BUTTON = 9 self.RIGHT_JOYSTICK_BUTTON = 10 self.BACK_BUTTON = 7 self.START_BUTTON = 8 self.rev_per_ft = 12 / (math.pi * self.wheel_diameter) self.br_motor = ctre.wpi_talonsrx.WPI_TalonSRX(5) self.bl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(4) self.fr_motor = ctre.wpi_talonsrx.WPI_TalonSRX(7) self.fl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(3) self.arm = ctre.wpi_talonsrx.WPI_TalonSRX(0) self.front_lift = ctre.wpi_talonsrx.WPI_TalonSRX(6) self.front_lift_slave = ctre.wpi_talonsrx.WPI_TalonSRX(50) self.front_lift_slave.follow(self.front_lift) self.back_lift = ctre.wpi_talonsrx.WPI_TalonSRX(2) self.back_lift_wheel = ctre.wpi_talonsrx.WPI_TalonSRX(1) self.fl_motor.setInverted(True) self.bl_motor.setInverted(True) self.arm.setInverted(True) self.fl_motor.configSelectedFeedbackSensor( ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS) self.bl_motor.configSelectedFeedbackSensor( ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS) self.fr_motor.configSelectedFeedbackSensor( ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS) self.br_motor.configSelectedFeedbackSensor( ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS) self.front_lift.configSelectedFeedbackSensor( ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS) self.back_lift.configSelectedFeedbackSensor( ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, MyRobot.TIMEOUT_MS) # Reverse negative encoder values self.fl_motor.setSensorPhase(True) #self.fr_motor.setSensorPhase(True) self.br_motor.setSensorPhase(True) self.front_lift.setSensorPhase(True) self.deadzone_amount = 0.15 self.control_state = "speed" self.start_navx = 0 self.previous_hyp = 0 self.js_startAngle = 0 self.rot_startNavx = 0 self.joystick = wpilib.Joystick(0) NetworkTables.addEntryListener(self.entry_listener) self.use_pid = False self.prev_pid_toggle_btn_value = False self.navx = navx.AHRS.create_spi() self.relativeGyro = RelativeGyro(self.navx) self.timer = wpilib.Timer() self.arm_pot = wpilib.AnalogPotentiometer(0) self.arm_pid = wpilib.PIDController(3, 0, 0, self.arm_pot.get, self.pid_output) self.init_time = 0 self.desired_rate = 0 self.pid_turn_rate = 0 self.prev_button1 = False self.button = False self.button_chomp = False self.front_lift_heights_index = 0 self.lift_target = 0 self.driveStates = { 'velocity': Velocty_Mode(self), 'enter_position': Enter_Position_Mode(self), 'position': Position_Mode(self), 'enter_rotation': Enter_Rotation_Mode(self), 'rotation': Rotation_Mode(self), 'leave_special': Leave_Special_Mode(self), 'lift_robot': Lift_Robot(self) } self.drive_sm = State_Machine(self.driveStates, "Drive_sm") self.drive_sm.set_state('velocity') self.liftStates = { 'fully_raised': Fully_Raised(self), 'middle': Middle(self), 'fully_lowered': Fully_Lowered(self), 'go_to_height': Go_To_Height(self) } self.lift_sm = State_Machine(self.liftStates, "lift_sm") self.lift_sm.set_state('fully_lowered') self.wheel_motors = [ self.br_motor, self.bl_motor, self.fr_motor, self.fl_motor ] wpilib.CameraServer.launch() def normalized_navx(): return self.get_normalized_angle(self.navx.getAngle()) self.angle_pid = wpilib.PIDController(self.turn_rate_p, self.turn_rate_i, self.turn_rate_d, self.relativeGyro.getAngle, self.set_pid_turn_rate) #self.turn_rate_pid. #self.turn_rate_pid. self.angle_pid.setInputRange(-self.turn_rate_pid_input_range, self.turn_rate_pid_input_range) self.angle_pid.setOutputRange(-self.turn_rate_pid_output_range, self.turn_rate_pid_output_range) self.angle_pid.setContinuous(True) self.turn_rate_values = [0] * 10 def pid_output(self, output): self.arm.set(output) def set_pid_turn_rate(self, turn_rate): self.pid_turn_rate = -turn_rate #print('turn_rate:', turn_rate) def get_normalized_angle(self, unnormalized_angle): angle = unnormalized_angle % 360 if angle > 180: return angle - 360 elif angle < -180: return angle + 360 else: return angle def entry_listener(self, key, value, is_new): try: if key == '/gyro/turn_rate_p': self.angle_pid.setP(self.turn_rate_p) elif key == '/gyro/turn_rate_i': self.angle_pid.setI(self.turn_rate_i) elif key == '/gyro/turn_rate_d': self.angle_pid.setD(self.turn_rate_d) if key == '/lifts/front_lift_heights_index': self.lift_target = self.front_lift_heights[int( self.front_lift_heights_index)] if "encoders" in key: self.set_wheel_pids() if 'elevator' in key: pass #self.setMotorPids(self.front_lift, self.elevator_p, self.elevator_i, self.elevator_d, self.elevator_f) except Exception as oopsy: print("There was an oopsy: " + str(oopsy)) def set_wheel_pids(self): if self.drive_sm.get_state() == 'position': for motor in self.wheel_motors: self.setMotorPids(motor, self.position_p, self.position_i, self.position_d, self.position_f) else: for motor in self.wheel_motors: self.setMotorPids(motor, self.velocity_p, self.velocity_i, self.velocity_d, self.velocity_f) def autonomousInit(self): self.periodInit() def teleopInit(self): self.periodInit() def periodInit(self): self.desired_angle = 0 self.navx.reset() self.angle_pid.disable() self.arm_pid.enable() self.arm_pid.setSetpoint(float(self.open_state)) self.front_lift.setQuadraturePosition(0, 0) self.fr_motor.setQuadraturePosition(0, 0) self.fl_motor.setQuadraturePosition(0, 0) self.br_motor.setQuadraturePosition(0, 0) self.bl_motor.setQuadraturePosition(0, 0) self.back_lift.setSelectedSensorPosition(0, 0, 0) def on_pid_toggle(self): """When button 4 is pressed, use_pid is toggled""" pid_toggle_btn_value = self.joystick.getRawButton(4) if not self.prev_pid_toggle_btn_value and pid_toggle_btn_value: self.use_pid = not self.use_pid print('PID changed to ' + str(self.use_pid)) self.prev_pid_toggle_btn_value = pid_toggle_btn_value def get_drive_cartesian(self): x_speed = self.deadzone(self.joystick.getRawAxis(self.LX_AXIS)) return driveCartesian( x_speed, -self.deadzone(self.joystick.getRawAxis(self.LY_AXIS)), self.deadzone(self.joystick.getRawAxis(self.RX_AXIS)), self.deadzone(self.relativeGyro.getAngle())) def teleopPeriodic(self): self.robotPeriodic() def autonomousPeriodic(self): self.robotPeriodic() def robotPeriodic(self): self.drive_sm.run() #print('state: ', self.drive_sm.get_state()) #self.lift_sm.run() #print("elevator pid: ", self.get_lift_position(), "lift state: ", self.lift_sm.get_state(), " target position: ",self.front_lift_heights[self.front_lift_heights_index]) # print(f"FL: {self.fl_motor.getQuadraturePosition()} FR: {self.fr_motor.getQuadraturePosition()} BL: {self.bl_motor.getQuadraturePosition()} BR: {self.br_motor.getQuadraturePosition()}") #print(f'p: {self.velocity_p} i: {self.velocity_i} d:{self.velocity_d} f: {self.velocity_f}') #print ('Pitch', self.navx.getPitch()) self.match_time = self.timer.getMatchTime() #print(f"FL: {self.fl_motor.getQuadraturePosition()} FR: {self.fr_motor.getQuadraturePosition()} BL: {self.bl_motor.getQuadraturePosition()} BR: {self.br_motor.getQuadraturePosition()}") def get_lift_position(self): return -self.front_lift.getQuadraturePosition() def regular_mec_drive(self): x = self.joystick.getRawAxis(0) y = self.joystick.getRawAxis(1) rot = self.joystick.getRawAxis(4) self_drive.mecanumDrive_Cartesian(self.dead_zone(x), self.dead_zone(y), self.dead_zone(rot), 0) def teleop_arms(self): if self.joystick.getRawAxis(2) > .2: self.spinman.set(-self.joystick.getRawAxis(2) * .5) elif self.joystick.getRawAxis(3) > .2: self.spinman.set(self.joystick.getRawAxis(3) * .5) else: self.spinman.set(0) #if self.joystick.getRawButton(4): # self.armsup = not self.armsup #if self.armsup: # self.littlearms1.set(self.liftforball + self.servo_offset1) # self.littlearms2.set(self.liftforball + self.servo_offset2) if self.joystick.getRawButton(2): self.littlearms1.set(self.arm_up + self.servo_offset1) self.littlearms2.set(self.arm_up + self.servo_offset2) else: self.littlearms1.set(self.arm_down + self.servo_offset1) self.littlearms2.set(self.arm_down + self.servo_offset2) print("Lil Arms 1: ", self.littlearms1.get(), "Lil Arms 2:", self.littlearms2.get()) def deadzone(self, value, min=.2): if -min < value < min: return 0 else: scaled_value = (abs(value) - min) / (1 - min) return math.copysign(scaled_value, value) def to_motor_speed(self, ft_per_second, ticks_per_rev): ticks_per_ft = ticks_per_rev * self.rev_per_ft ticks_per_sec = ft_per_second * ticks_per_ft return ticks_per_sec * .1
class MyRobot(wpilib.TimedRobot): """Main robot class""" # NetworkTables API for controlling this #: Speed to set the motors to autospeed = ntproperty("/robot/autospeed", defaultValue=0, writeDefault=True) #: Test data that the robot sends back telemetry = ntproperty( "/robot/telemetry", defaultValue=(0,) * 6, writeDefault=False ) prior_autospeed = 0 #: The total gear reduction between the encoder and the arm GEARING = 1 #: The offset of encoder zero from horizontal, in degrees. #: It is CRUCIAL that this be set correctly, or the characterization will not work! OFFSET = 0 ENCODER_PULSE_PER_REV = 360 def robotInit(self): """Robot-wide initialization code should go here""" self.lstick = wpilib.Joystick(0) self.arm_motor = wpilib.Spark(1) self.arm_motor.setInverted(False) # # Configure encoder related functions -- getpos and getrate should return # degrees and degrees/s # encoder_constant = ( (1 / self.ENCODER_PULSE_PER_REV) / self.GEARING * 360 ) encoder = wpilib.Encoder(0, 1) encoder.setDistancePerPulse(encoder_constant) self.encoder_getpos = (lambda: encoder.getDistance() + self.OFFSET) self.encoder_getrate = encoder.getRate # Set the update rate instead of using flush because of a NetworkTables bug # that affects ntcore and pynetworktables # -> probably don't want to do this on a robot in competition NetworkTables.setUpdateRate(0.010) def disabledInit(self): self.logger.info("Robot disabled") self.arm_motor.set(0) def disabledPeriodic(self): pass def robotPeriodic(self): # feedback for users, but not used by the control program sd = wpilib.SmartDashboard sd.putNumber("encoder_pos", self.encoder_getpos()) sd.putNumber("encoder_rate", self.encoder_getrate()) def teleopInit(self): self.logger.info("Robot in operator control mode") def teleopPeriodic(self): self.arm_motor.set(-self.lstick.getY()) def autonomousInit(self): self.logger.info("Robot in autonomous mode") def autonomousPeriodic(self): """ If you wish to just use your own robot program to use with the data logging program, you only need to copy/paste the logic below into your code and ensure it gets called periodically in autonomous mode Additionally, you need to set NetworkTables update rate to 10ms using the setUpdateRate call. Note that reading/writing self.autospeed and self.telemetry are NetworkTables operations (using pynetworktables's ntproperty), so if you don't read/write NetworkTables in your implementation it won't actually work. """ # Retrieve values to send back before telling the motors to do something now = wpilib.Timer.getFPGATimestamp() encoder_position = self.encoder_getpos() encoder_rate = self.encoder_getrate() battery = self.ds.getBatteryVoltage() motor_volts = battery * abs(self.prior_autospeed) # Retrieve the commanded speed from NetworkTables autospeed = self.autospeed self.prior_autospeed = autospeed # command motors to do things self.arm_motor.set(autospeed) # send telemetry data array back to NT self.telemetry = ( now, battery, autospeed, motor_volts, encoder_position, encoder_rate )