def read(self, first_address, count):
data = [first_address, count]
data.append(crc7(data))
with self.mutex:
retcount = self.port.write(data)
if retcount != len(data):
raise IOError("Write error (%s != %s)" % (retcount, len(data)))
# FIXME
if not hal.isSimulation():
Timer.delay(0.001)
data = self.port.read(True, count + 1)
if len(data) != count + 1:
raise IOError("Read error (%s != %s)" % (len(data), count+1))
crc = data[-1]
data = data[:-1]
if crc7(data) != crc:
raise IOError("CRC error")
return data
def initSendable(self, builder: SendableBuilder) -> None:
builder.setSmartDashboardType("String Chooser")
builder.getEntry(SendableChooser.INSTANCE).setDouble(self.instance)
builder.addStringProperty(self.DEFAULT, lambda: self.defaultChoice,
None)
builder.addStringArrayProperty(self.OPTIONS, lambda: self.map.keys(),
None)
def _active_property_getter():
with self.mutex:
return self.selected if self.selected else self.defaultChoice
builder.addStringProperty(SendableChooser.ACTIVE,
_active_property_getter, None)
with self.mutex:
self.activeEntries.append(builder.getEntry(SendableChooser.ACTIVE))
def _selected_property_setter(val):
with self.mutex:
self.selected = val
for entry in self.activeEntries:
entry.setString(val)
# python-specific: set local=True
builder.addStringProperty(
SendableChooser.SELECTED,
None,
_selected_property_setter,
local=hal.isSimulation(), # only need local updates in simulation
)
def __init__(self):
'''creates two controllers and assigns
axis and buttons to joysticks'''
driverController = {}
auxController = {}
driverController['controllerId'] = 0
auxController['controllerId'] = 1
driverController['leftTread'] = 1
driverController['leftRot'] = 0
if hal.isSimulation():
driverController['rightTread'] = 3
else:
driverController['rightTread'] = 5
'''Create buttons for the drive controller'''
driverController['ledToggle'] = wpilib.XboxController.Button.kX
driverController['alignButton'] = wpilib.XboxController.Button.kA
driverController['leftButton'] = wpilib.XboxController.Button.kBumperLeft
driverController['rightButton'] = wpilib.XboxController.Button.kBumperRight
driverController['straightButton'] = wpilib.XboxController.Button.kY
'''Create buttons for the aux controller'''
auxController['LowerButton'] = wpilib.XboxController.Button.kBumperLeft
auxController['RaiseButton'] = wpilib.XboxController.Button.kBumperRight
auxController['StopButton'] = wpilib.XboxController.Button.kY
auxController['PistonButton'] = wpilib.XboxController.Button.kX
auxController['RollerIO'] = wpilib.XboxController.Button.kA
auxController['RollerToggle'] = wpilib.XboxController.Button.kB
driverController['voltRumble'] = 8.0
self.driverController = driverController
self.auxController = auxController
def initSendable(self, builder: SendableBuilder) -> None:
builder.setSmartDashboardType("String Chooser")
builder.getEntry(SendableChooser.INSTANCE).setDouble(self.instance)
builder.addStringProperty(self.DEFAULT, lambda: self.defaultChoice, None)
builder.addStringArrayProperty(self.OPTIONS, lambda: self.map.keys(), None)
def _active_property_getter():
with self.mutex:
return self.selected if self.selected else self.defaultChoice
builder.addStringProperty(SendableChooser.ACTIVE, _active_property_getter, None)
with self.mutex:
self.activeEntries.append(builder.getEntry(SendableChooser.ACTIVE))
def _selected_property_setter(val):
with self.mutex:
self.selected = val
for entry in self.activeEntries:
entry.setString(val)
# python-specific: set local=True
builder.addStringProperty(
SendableChooser.SELECTED,
None,
_selected_property_setter,
local=hal.isSimulation(), # only need local updates in simulation
)
def __init__(self, port: wpilib.I2C.Port = None, address: int = None) -> None:
super().__init__()
if address is None:
address = self.ADDRESS_A
if port is None:
port = wpilib.I2C.Port.kMXP
sim_port = None
if hal.isSimulation():
from .bno055_sim import BNO055Sim
sim_port = BNO055Sim()
self.i2c = wpilib.I2C(port, address, sim_port)
# set the units that we want
self.offset = 0.0
current_units = self.i2c.read(self.Register.UNIT_SEL, 1)[0]
for wanted, index in self.UNIT_SEL_LIST:
if wanted == 1:
current_units = current_units | (1 << index)
elif wanted == 0:
current_units = current_units & ~(1 << index)
self.i2c.write(self.Register.UNIT_SEL, current_units)
self.setOperationMode(self.OperationMode.IMUPLUS) # accelerometer and gyro
self.reverse_axis(False, False, False)
self.cache_heading()
def read(self, first_address, count):
data = [first_address, count]
data.append(crc7(data))
with self.mutex:
retcount = self.port.write(data)
if retcount != len(data):
raise IOError("Write error (%s != %s)" % (retcount, len(data)))
# FIXME
if not hal.isSimulation():
Timer.delay(0.001)
data = self.port.read(True, count + 1)
if len(data) != count + 1:
raise IOError("Read error (%s != %s)" % (len(data), count + 1))
crc = data[-1]
data = data[:-1]
if crc7(data) != crc:
raise IOError("CRC error")
return data
def main(robot_cls):
"""Starting point for the applications."""
RobotBase.initializeHardwareConfiguration()
hal.report(hal.UsageReporting.kResourceType_Language,
hal.UsageReporting.kLanguage_Python)
try:
robot = robot_cls()
except:
from .driverstation import DriverStation
DriverStation.reportError(
"Unhandled exception instantiating robot " +
robot_cls.__name__, True)
DriverStation.reportWarning(
"Robots should not quit, but yours did!", False)
DriverStation.reportError(
"Could not instantiate robot " + robot_cls.__name__ + "!",
False)
return False
# Add a check to see if the user forgot to call super().__init__()
if not hasattr(robot, '_RobotBase__initialized'):
logger.error(
"If your robot class has an __init__ function, it must call super().__init__()!"
)
return False
if not hal.isSimulation():
try:
import wpilib
with open('/tmp/frc_versions/FRC_Lib_Version.ini', 'w') as fp:
fp.write('RobotPy %s' % wpilib.__version__)
except:
logger.warning("Could not write FRC version file to disk")
try:
robot.startCompetition()
except KeyboardInterrupt:
logger.exception(
"THIS IS NOT AN ERROR: The user hit CTRL-C to kill the robot")
logger.info("Exiting because of keyboard interrupt")
return True
except:
from .driverstation import DriverStation
DriverStation.reportError("Unhandled exception", True)
DriverStation.reportWarning(
"Robots should not quit, but yours did!", False)
DriverStation.reportError(
"The startCompetition() method (or methods called by it) should have handled the exception above.",
False)
return False
else:
# startCompetition never returns unless exception occurs....
from .driverstation import DriverStation
DriverStation.reportWarning(
"Robots should not quit, but yours did!", False)
DriverStation.reportError(
"Unexpected return from startCompetition() method.", False)
return False
def launch(cls, vision_py=None):
"""
Launches the CameraServer process in autocapture mode or
using a user-specified python script
:param vision_py: If specified, this is the relative path to
a filename with a function in it
Example usage::
wpilib.CameraServer.launch("vision.py:main")
.. warning:: You must have robotpy-cscore installed, or this
function will fail without returning an error
(you will see an error in the console).
"""
if cls._launched:
return
cls._launched = True
if hal.isSimulation():
logger.info("Would launch CameraServer with vision_py=%s",
vision_py)
cls._alive = True
else:
logger.info("Launching CameraServer process")
# Launch the cscore launcher in a separate process
import subprocess
import sys
args = [sys.executable, "-m", "cscore"]
# TODO: Get accurate reporting data from the other cscore process. For
# now, just differentiate between users with a custom py file and those
# who do not. cscore handle values indicate type with bits 24-30
if vision_py:
if not vision_py.startswith("/"):
vision_py = "/home/lvuser/py/" + vision_py
args.append(vision_py)
hal.report(hal.UsageReporting.kResourceType_PCVideoServer,
0x51)
else:
hal.report(hal.UsageReporting.kResourceType_PCVideoServer,
0x52)
# We open a pipe to it so that when this process exits, it dies
proc = subprocess.Popen(args,
close_fds=True,
stdin=subprocess.PIPE,
cwd="/home/lvuser/py")
th = threading.Thread(target=cls._monitor_child, args=(proc, ))
th.daemon = True
th.start()
def __init__(self, talon: TalonSRX, frame_period: int, min_points=20):
self.talon = talon
if not hal.isSimulation():
self.talon.configMotionProfileTrajectoryPeriod(frame_period, 0)
self.min_points = min_points
self.frame_period = frame_period
self._process_mp_thread = threading.Thread(target=self._process_mp)
def initialize(self):
poz = pose_estimator.get_current_pose()
self.target_angle = (self.lookat - poz).angle() * 180 / math.pi
if hal.isSimulation():
from pyfrc.sim import get_user_renderer
render = get_user_renderer()
if render is not None: # If running the standalone (pyplot) sim, this is None
render.draw_line([(poz.x, -poz.y + 14), (self.lookat.x, -self.lookat.y + 14)], color="#00ff00", arrow=False)
def drive_straight(self, speed=0.25):
self.turn_controller.enable()
#Stay straight
self.turn_controller.setSetpoint(0)
if not hal.isSimulation():
self.arcade_drive(self.pid_value, speed)
else:
self.arcade_drive(0, speed)
def __init__(self):
super().__init__()
if hal.isSimulation():
self.sensor = Mock()
self.sensor.getAverageVoltage = lambda: 0
else:
self.sensor = AnalogInput(Constants.DISTANCE_SENSOR_PORT)
self.sensor.setAverageBits(4)
def init(self):
if hal.isSimulation():
self.sensor = Mock()
self.sensor.getAverageVoltage = lambda: 0
self.sensor.getAccumulatorCount = lambda: 0
self.sensor.resetAccumulator = lambda: 0
else:
self.sensor = AnalogInput(Constants.DISTANCE_SENSOR_PORT)
self.sensor.setAverageBits(4)
def __init__(self, port: Optional[SPI.Port] = None) -> None:
"""
Constructor.
:param port: The SPI port that the gyro is connected to
"""
super().__init__()
if port is None:
port = SPI.Port.kOnboardCS0
simPort = None
if hal.HALIsSimulation():
from hal_impl.spi_helpers import ADXRS450_Gyro_Sim
simPort = ADXRS450_Gyro_Sim(self)
self.spi = SPI(port, simPort=simPort)
self.spi.setClockRate(3000000)
self.spi.setMSBFirst()
self.spi.setSampleDataOnLeadingEdge()
self.spi.setClockActiveHigh()
self.spi.setChipSelectActiveLow()
# Validate the part ID
if (self._readRegister(self.kPIDRegister) & 0xFF00) != 0x5200:
self.spi.close()
self.spi = None
DriverStation.reportError(
"could not find ADXRS450 gyro on SPI port %s" % port, False
)
return
# python-specific: make this easier to simulate
if hal.isSimulation():
self.spi.initAccumulator(
self.kSamplePeriod, 0x20000000, 4, 0x0, 0x0, 0, 32, True, True
)
else:
self.spi.initAccumulator(
self.kSamplePeriod,
0x20000000,
4,
0x0C00000E,
0x04000000,
10,
16,
True,
True,
)
self.calibrate()
hal.report(hal.UsageReporting.kResourceType_ADXRS450, port)
self.setName("ADXRS450_Gyro", port)
def __init__(self):
# Init I2C for communication with Arduino
if (hal.isSimulation()):
# import stub for simulation
from components.i2cstub import I2CStub
self.arduinoC = wpilib.I2C(wpilib.I2C.Port.kOnboard, 4, I2CStub())
else:
self.arduinoC = wpilib.I2C(wpilib.I2C.Port.kOnboard, 4)
self.allianceColor = 'red'
def __init__(self, port=None):
"""
Constructor.
:param port: The SPI port that the gyro is connected to
:type port: :class:`.SPI.Port`
"""
super().__init__()
if port is None:
port = SPI.Port.kOnboardCS0
simPort = None
if hal.HALIsSimulation():
from hal_impl.spi_helpers import ADXRS450_Gyro_Sim
simPort = ADXRS450_Gyro_Sim(self)
self.spi = SPI(port, simPort=simPort)
self.spi.setClockRate(3000000)
self.spi.setMSBFirst()
self.spi.setSampleDataOnLeadingEdge()
self.spi.setClockActiveHigh()
self.spi.setChipSelectActiveLow()
# Validate the part ID
if (self._readRegister(self.kPIDRegister) & 0xFF00) != 0x5200:
self.spi.close()
self.spi = None
DriverStation.reportError(
"could not find ADXRS450 gyro on SPI port %s" % port, False)
return
# python-specific: make this easier to simulate
if hal.isSimulation():
self.spi.initAccumulator(self.kSamplePeriod, 0x20000000, 4, 0x0,
0x0, 0, 32, True, True)
else:
self.spi.initAccumulator(
self.kSamplePeriod,
0x20000000,
4,
0x0C00000E,
0x04000000,
10,
16,
True,
True,
)
self.calibrate()
hal.report(hal.UsageReporting.kResourceType_ADXRS450, port)
self.setName("ADXRS450_Gyro", port)
def initialize(self):
pose_estimator.set_new_pose(
Pose(x=1.5,
y=-10 *
(1 if game_data.get_robot_side() == Side.RIGHT else -1),
heading=0))
self.group = get_scale_only_group(self.drive, self.elevator,
self.intake)
self.group.addSequential(PrintCommand("Done with only scale"))
if not hal.isSimulation():
self.group.addParallel(MoveElevatorCommand(self.elevator, 0))
else:
self.group.addParallel(PrintCommand("Elevator moving"))
on_left = game_data.get_scale_side() == Side.LEFT
self.group.addSequential(
TurnToLookat(self.drive,
lookat=Vector2(16, 5 * (1 if on_left else -1))))
self.group.addSequential(
AcquireCube(drive=self.drive,
drive_speed=0.4,
intake=self.intake,
timeout=0.3))
if game_data.get_scale_side() == game_data.get_own_switch_side():
if not hal.isSimulation():
self.group.addSequential(MoveElevatorCommand(
self.elevator, 30))
self.group.addSequential(
MoveIntakeCommand(self.intake, ArmState.DOWN))
else:
self.group.addParallel(PrintCommand("Elevator moving"))
self.group.addSequential(
DistanceDriveCommand(drive=self.drive,
power=0.25,
distance=0.5))
self.group.addSequential(PrintCommand("Distance done"))
self.group.addSequential(
SetIntakeCommand(intake=self.intake, new_state=GrabState.OUT))
self.group.start()
def launch(cls, vision_py: Optional[str] = None) -> None:
"""
Launches the CameraServer process in autocapture mode or
using a user-specified python script
:param vision_py: If specified, this is the relative path to
a filename with a function in it
Example usage::
wpilib.CameraServer.launch("vision.py:main")
.. warning:: You must have robotpy-cscore installed, or this
function will fail without returning an error
(you will see an error in the console).
"""
if cls._launched:
return
cls._launched = True
if hal.isSimulation():
logger.info("Would launch CameraServer with vision_py=%s", vision_py)
cls._alive = True
else:
logger.info("Launching CameraServer process")
# Launch the cscore launcher in a separate process
import sys
args = [sys.executable, "-m", "cscore"]
# TODO: Get accurate reporting data from the other cscore process. For
# now, just differentiate between users with a custom py file and those
# who do not. cscore handle values indicate type with bits 24-30
if vision_py:
if not vision_py.startswith("/"):
vision_py = "/home/lvuser/py/" + vision_py
args.append(vision_py)
hal.report(hal.UsageReporting.kResourceType_PCVideoServer, 0x51)
else:
hal.report(hal.UsageReporting.kResourceType_PCVideoServer, 0x52)
# We open a pipe to it so that when this process exits, it dies
proc = subprocess.Popen(
args, close_fds=True, stdin=subprocess.PIPE, cwd="/home/lvuser/py"
)
th = threading.Thread(target=cls._monitor_child, args=(proc,))
th.daemon = True
th.start()
def execute(self):
if self.reversed:
self.left_talon0.set(-self.right * self.drive_multiplier.value)
if hal.isSimulation():
self.right_talon0.set(-self.left * self.drive_multiplier.value)
else:
self.right_talon0.set(-self.left * self.drive_multiplier.value)
else:
#0.94375
self.left_talon0.set(self.left * self.drive_multiplier.value)
if hal.isSimulation():
self.right_talon0.set(self.right * self.drive_multiplier.value)
else:
self.right_talon0.set(self.right * self.drive_multiplier.value)
#Reset left and right to 0
self.left = 0
self.right = 0
#Update SD
self._update_sd()
def launch(cls, vision_py=None):
'''
Launches the CameraServer process in autocapture mode or
using a user-specified python script
:param vision_py: If specified, this is the relative path to
a filename with a function in it
Example usage::
wpilib.CameraServer.launch("vision.py:main")
.. warning:: You must have robotpy-cscore installed, or this
function will fail without returning an error
(you will see an error in the console).
'''
if cls._launched:
return
cls._launched = True
if hal.isSimulation():
logger.info("Would launch CameraServer with vision_py=%s",
vision_py)
cls._alive = True
else:
logger.info("Launching CameraServer process")
# Launch the cscore launcher in a separate process
import subprocess
import sys
args = [sys.executable, '-m', 'cscore']
if vision_py:
if not vision_py.startswith('/'):
vision_py = '/home/lvuser/py/' + vision_py
args.append(vision_py)
# We open a pipe to it so that when this process exits, it dies
proc = subprocess.Popen(args,
close_fds=True,
stdin=subprocess.PIPE,
cwd='/home/lvuser/py')
th = threading.Thread(target=cls._monitor_child, args=(proc, ))
th.daemon = True
th.start()
def setup(self):
"""This is called after variables are injected by magicbot."""
self.set_pos = None
self.motor.setQuadraturePosition(0, timeoutMs=10)
self.motor.setInverted(True)
self.motor.setNeutralMode(ctre.NeutralMode.Brake)
self.motor.overrideLimitSwitchesEnable(False)
self.motor.configReverseLimitSwitchSource(
ctre.TalonSRX.LimitSwitchSource.FeedbackConnector,
ctre.TalonSRX.LimitSwitchNormal.NormallyOpen,
deviceID=0,
timeoutMs=10)
self.motor.configForwardLimitSwitchSource(
ctre.TalonSRX.LimitSwitchSource.Deactivated,
ctre.TalonSRX.LimitSwitchNormal.Disabled,
deviceID=0,
timeoutMs=10)
self.motor.overrideSoftLimitsEnable(True)
self.motor.configForwardSoftLimitEnable(True, timeoutMs=10)
self.motor.configForwardSoftLimitThreshold(self.metres_to_counts(
self.TOP_HEIGHT),
timeoutMs=10)
self.motor.configReverseSoftLimitEnable(False, timeoutMs=10)
if not hal.isSimulation():
self.motor.configSetParameter(
ctre.TalonSRX.ParamEnum.eClearPositionOnLimitR,
1,
0,
0,
timeoutMs=10)
self.motor.configSelectedFeedbackSensor(
ctre.FeedbackDevice.QuadEncoder, 0, timeoutMs=10)
# TODO tune motion profiling
self.motor.selectProfileSlot(0, 0)
self.motor.config_kF(0, 1023 / self.FREE_SPEED, timeoutMs=10)
self.motor.config_kP(0, 2, timeoutMs=10)
self.motor.config_kI(0, 0, timeoutMs=10)
self.motor.config_kD(0, 1, timeoutMs=10)
self.motor.configMotionAcceleration(self.UPWARD_ACCELERATION,
timeoutMs=10)
self.motor.configMotionCruiseVelocity(int(self.FREE_SPEED),
timeoutMs=10)
def execute(self):
pid_z = 0
if self.hold_heading:
pid_z = self.heading_pid.get()
vz = self.vz + pid_z
for module in self.modules:
module_dist = math.hypot(module.x_pos, module.y_pos)
module_angle = math.atan2(module.y_pos, module.x_pos)
# Calculate the additional vx and vy components for this module
# required to achieve our desired angular velocity
vz_x = -module_dist * vz * math.sin(module_angle)
vz_y = module_dist * vz * math.cos(module_angle)
# TODO: re enable this and test field-oriented mode
if self.field_oriented:
if hal.isSimulation():
from hal_impl.data import hal_data
angle = math.radians(-hal_data['robot']['bno055'])
else:
angle = self.bno055.getAngle()
vx, vy = self.field_orient(self.vx, self.vy, angle)
else:
vx, vy = self.vx, self.vy
module.set_velocity(vx + vz_x, vy + vz_y)
odometry_outputs = np.zeros((8, 1))
velocity_outputs = np.zeros((8, 1))
for i, module in enumerate(self.modules):
odometry_x, odometry_y = module.get_cartesian_delta()
velocity_x, velocity_y = module.get_cartesian_vel()
odometry_outputs[i * 2, 0] = odometry_x
odometry_outputs[i * 2 + 1, 0] = odometry_y
velocity_outputs[i * 2, 0] = velocity_x
velocity_outputs[i * 2 + 1, 0] = velocity_y
module.reset_encoder_delta()
delta_x, delta_y, delta_theta = self.robot_movement_from_odometry(
odometry_outputs)
v_x, v_y, v_z = self.robot_movement_from_odometry(velocity_outputs)
self.odometry_x += delta_x
self.odometry_y += delta_y
self.odometry_theta += delta_theta
self.odometry_x_vel = v_x
self.odometry_y_vel = v_y
self.odometry_z_vel = v_z
def _log_versions():
import wpilib
import hal
import hal_impl
import logging
logger = logging.getLogger("wpilib")
logger.info("WPILib version %s", wpilib.__version__)
logger.info(
"HAL base version %s; %s platform version %s",
hal.__version__,
hal_impl.__halplatform__,
hal_impl.__version__,
)
if hasattr(hal_impl.version, "__hal_version__"):
logger.info("HAL library version %s", hal_impl.version.__hal_version__)
# should we just die here?
if (
hal.__version__ != wpilib.__version__
and hal.__version__ != hal_impl.__version__
):
logger.warning(
"Core component versions are not identical! This is not a supported configuration, and you may run into errors!"
)
if hal.isSimulation():
logger.info("Running with simulated HAL.")
# check to see if we're on a RoboRIO
# NOTE: may have false positives, but it should work well enough
if exists("/etc/natinst/share/scs_imagemetadata.ini"):
logger.warning(
"Running simulation HAL on actual roboRIO! This probably isn't what you want, and will probably cause difficult-to-debug issues!"
)
# Log third party versions
# -> TODO: in the future, expand 3rd party HAL support here?
for entry_point in iter_entry_points(group="robotpylib", name=None):
# Don't actually load the entry points -- just print the
# packages unless we need to load them
dist = entry_point.dist
logger.info("%s version %s", dist.project_name, dist.version)
def __init__(self):
'''creates two controllers for driver and shooter and assigns
axis and buttons to joysticks'''
driverController = {}
auxController = {}
driverController['controllerId'] = 0
driverController['leftTread'] = 1
if hal.isSimulation():
driverController['rightTread'] = 3
else:
driverController['rightTread'] = 5
driverController['voltRumble'] = 8.0
self.driverController = driverController
self.auxController = auxController
def execute(self):
for module in self.modules:
module_dist = math.hypot(module.x_pos, module.y_pos)
module_angle = math.atan2(module.y_pos, module.x_pos)
# Calculate the additional vx and vy components for this module
# required to achieve our desired angular velocity
vz_x = -module_dist * self.vz * math.sin(module_angle)
vz_y = module_dist * self.vz * math.cos(module_angle)
# TODO: re enable this and test field-oriented mode
if self.field_oriented:
if hal.isSimulation():
from hal_impl.data import hal_data
angle = math.radians(-hal_data['robot']['bno055'])
else:
angle = self.bno055.getAngle()
vx, vy = self.field_orient(self.vx, self.vy, angle)
else:
vx, vy = self.vx, self.vy
module.set_velocity(vx + vz_x, vy + vz_y)
def __init__(self) -> None:
self.last_pong = time.monotonic()
# 50Hz control loop for 2 seconds
self.odometry: Deque[Odometry] = deque(maxlen=50 * 2)
self.ntinst = NetworkTablesInstance()
if hal.isSimulation():
self.ntinst.startTestMode(server=False)
else:
self.ntinst.startClient("10.47.74.6") # Raspberry pi's IP
self.ntinst.setUpdateRate(1) # ensure our flush calls flush immediately
self.fiducial_x_entry = self.ntinst.getEntry("/vision/fiducial_x")
self.fiducial_y_entry = self.ntinst.getEntry("/vision/fiducial_y")
self.fiducial_time_entry = self.ntinst.getEntry("/vision/fiducial_time")
self.ping_time_entry = self.ntinst.getEntry("/vision/ping")
self.raspi_pong_time_entry = self.ntinst.getEntry("/vision/raspi_pong")
self.rio_pong_time_entry = self.ntinst.getEntry("/vision/rio_pong")
self.latency_entry = self.ntinst.getEntry("/vision/clock_offset")
self.processing_time_entry = self.ntinst.getEntry("/vision/processing_time")
self.camera_entry = self.ntinst.getEntry("/vision/game_piece")
def __init__(self):
driveController = {}
driveController['controllerId'] = 0
driveController['leftTread'] = 1
if hal.isSimulation():
driveController['rightTread'] = 3 #3 on sim 5 on frc
else:
driveController['rightTread'] = 5 #3 on sim 5 on frc
driveController['voltRumble'] = 8
auxController = {}
auxController['controllerId'] = 1
auxController['loaderToggleButton'] = 1
driveController['voltRumble'] = 8
#auxController['lifterDownAxis']= 4
#auxController['lifterUpAxis']=2
auxController['lifterDownAxis'] = 2
auxController['lifterUpAxis'] = 3
self.driveController = driveController
self.auxController = auxController
def initialize(self):
self.pp_controller.init()
print("Started pursuit")
cur_pose = pose_estimator.get_current_pose()
poz = pose_estimator.get_current_pose()
line_pts = []
for t in range(1000):
t0 = t / 1000
pt = self.pp_controller.path.spline.get_point(t0)
line_pts += [(pt.x, -pt.y + 14)]
dashboard2.draw(
list(map(lambda x: Vector2(x[0], -(x[1] - 14)), line_pts)))
if hal.isSimulation():
from pyfrc.sim import get_user_renderer
render = get_user_renderer()
render.draw_line(line_pts, robot_coordinates=False)
dashboard2.add_graph("CTE", self.pp_controller.get_cte)
dashboard2.add_graph("Lookahead",
lambda: self.pp_controller.current_lookahead)
dashboard2.add_graph("Goal Distance", lambda: self.goal_dist)
dashboard2.add_graph("Speed", lambda: self.speed)
def get_scale_only_group(drive, elevator, intake):
group = CommandGroup()
is_close = game_data.get_robot_side() == game_data.get_scale_side()
if game_data.get_robot_side() == Side.LEFT:
if is_close:
drive_command = close_drive_flipped
else:
drive_command = far_drive_flipped
else:
if is_close:
drive_command = close_drive
else:
drive_command = far_drive
elev_wait = TimedCommand(name="Elev Timeout",
timeoutInSeconds=(0.5 if is_close else 2))
elevator_to_height = MoveElevatorCommand(elevator, 60)
elev_group = CommandGroup()
elev_group.addSequential(elev_wait)
if not hal.isSimulation():
elev_group.addSequential(elevator_to_height)
else:
elev_group.addSequential(PrintCommand("Elevator moving"))
drop_cube = SetIntakeCommand(intake, GrabState.OUT)
drive_back = DistanceDriveCommand(drive=drive, power=-0.2, distance=3)
group.addParallel(elev_group)
group.addSequential(drive_command)
group.addSequential(drop_cube)
group.addSequential(drive_back)
return group
def __init__(self, drive: Drivetrain, elevator: Elevator, intake: Intake):
super().__init__("ScaleOnly command")
close_waypoints = [
Vector2(0, -10),
Vector2(16, -10),
Vector2(23, -7.5)
]
far_waypoints = [
Vector2(0, -10),
Vector2(20, -10),
Vector2(20, 7),
Vector2(23, 7.5)
]
cruise = 0.6
acc = 0.6
margin = 3 / 12
lookahead = 2
drive_path_far = PursuitDriveCommand(acc=acc,
cruise_speed=cruise,
waypoints=far_waypoints,
drive=drive,
dist_margin=margin,
lookahead_base=lookahead)
drive_path_close = PursuitDriveCommand(acc=acc,
cruise_speed=cruise,
waypoints=close_waypoints,
drive=drive,
dist_margin=margin,
lookahead_base=lookahead)
drive_path_far_flipped = PursuitDriveCommand(
acc=acc,
cruise_speed=cruise,
waypoints=pursuit.flip_waypoints_y(far_waypoints),
drive=drive,
dist_margin=margin,
lookahead_base=lookahead)
drive_path_close_flipped = PursuitDriveCommand(
acc=acc,
cruise_speed=cruise,
waypoints=pursuit.flip_waypoints_y(close_waypoints),
drive=drive,
dist_margin=margin,
lookahead_base=lookahead)
drive_path_chooser = ConditionalCommand(
"StandardScaleOnlySideCondition")
drive_path_chooser.onFalse = drive_path_far
drive_path_chooser.onTrue = drive_path_close
drive_path_chooser.condition = lambda: game_data.get_scale_side(
) == game_data.Side.RIGHT
drive_path_flip_chooser = ConditionalCommand(
"FlipScaleOnlySideCondition")
drive_path_flip_chooser.onFalse = drive_path_close_flipped
drive_path_flip_chooser.onTrue = drive_path_far_flipped
drive_path_flip_chooser.condition = lambda: game_data.get_scale_side(
) == game_data.Side.RIGHT
drive_flip_chooser = ConditionalCommand("DriveFlipCondition")
drive_flip_chooser.condition = lambda: game_data.get_robot_side(
) == game_data.Side.RIGHT
drive_flip_chooser.onTrue = drive_path_chooser
drive_flip_chooser.onFalse = drive_path_flip_chooser
elevator_condition = WaitUntilConditionCommand(
lambda: pose_estimator.get_current_pose().x > 16)
elevator_to_height = MoveElevatorCommand(elevator,
ElevatorPositions.SWITCH)
elev_group = CommandGroup()
elev_group.addSequential(elevator_condition)
if not hal.isSimulation():
elev_group.addSequential(elevator_to_height)
else:
elev_group.addSequential(PrintCommand("Elevator moving"))
intake_out = MoveIntakeCommand(intake, ArmState.DOWN)
drop_cube = TimedRunIntakeCommand(intake,
time=0.5,
power=-intake.power)
drive_back = TimeDriveCommand(drive, time=0.2, power=-0.5)
spin_chooser = ConditionalCommand("SpinChooser")
spin_chooser.condition = lambda: game_data.get_scale_side(
) == game_data.Side.RIGHT
spin_chooser.onTrue = TurnToAngle(drive, 180, delta=False)
spin_chooser.onFalse = TurnToAngle(drive, -180, delta=False)
switch_path_close = [Vector2(20, -10), Vector2(19, -8)]
switch_path_far = [Vector2(20, -10), Vector2(18, 7)]
switch_path_chooser = ConditionalCommand("SwitchPathChooser")
switch_path_chooser.condition = lambda: game_data.get_own_switch_side(
) == game_data.get_robot_side()
switch_path_chooser.onTrue = PursuitDriveCommand(
drive, switch_path_close, cruise, acc)
switch_path_chooser.onFalse = PursuitDriveCommand(
drive, switch_path_far, cruise, acc)
self.addParallel(elev_group)
self.addSequential(drive_flip_chooser)
self.addSequential(intake_out)
self.addSequential(drop_cube)
self.addSequential(drive_back)
if not hal.isSimulation():
self.addParallel(MoveElevatorCommand(elevator, 0))
else:
self.addParallel(PrintCommand("Elevator moving"))
self.addSequential(spin_chooser)
self.addSequential(switch_path_chooser)
import hal
from collections import namedtuple
from .faults import Faults
from .stickyfaults import StickyFaults
from .canifierfaults import CANifierFaults
from .canifierstickyfaults import CANifierStickyFaults
from .pigeonfaults import PigeonIMU_Faults
from .pigeonstickyfaults import PigeonIMU_StickyFaults
from .motionprofilestatus import MotionProfileStatus
if hal.isSimulation():
from .autogen.canifier_sim import CANifier
from .autogen.motcontroller_sim import MotController
from .autogen.pigeonimu_sim import PigeonIMU
from .autogen import ctre_sim_enums as _enum_module
else:
from .ctre_roborio import (CANifier, MotController, PigeonIMU)
from . import ctre_roborio as _enum_module
# enums
ControlMode = _enum_module.ControlMode
DemandType = _enum_module.DemandType
ErrorCode = _enum_module.ErrorCode
NeutralMode = _enum_module.NeutralMode
RemoteFeedbackDevice = _enum_module.RemoteFeedbackDevice
RemoteSensorSource = _enum_module.RemoteSensorSource
FeedbackDevice = _enum_module.FeedbackDevice
FollowerType = _enum_module.FollowerType
StatusFrame = _enum_module.StatusFrame
StatusFrameEnhanced = _enum_module.StatusFrameEnhanced
VelocityMeasPeriod = _enum_module.VelocityMeasPeriod
def isReal() -> bool:
"""Get if the robot is real.
:returns: If the robot is running in the real world.
"""
return not hal.isSimulation()
def isSimulation() -> bool:
"""Get if the robot is a simulation.
:returns: If the robot is running in simulation.
"""
return hal.isSimulation()
def getQuadraturePosition(self):
# 1/-1 for simulation and practice robot, -1/1 on comp bot
reverse = 1 if hal.isSimulation() else -1
return (reverse
if self.phase else -reverse) * super().getQuadraturePosition()