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
)
