def robotInit(self):
constants.load_control_config()
wpilib.CameraServer.launch('driver_vision.py:main')
self.autoPositionSelect = wpilib.SendableChooser()
self.autoPositionSelect.addDefault('Middle', 'Middle')
self.autoPositionSelect.addObject('Left', 'Left')
self.autoPositionSelect.addObject('Right', 'Right')
wpilib.SmartDashboard.putData('Robot Starting Position',
self.autoPositionSelect)
self.control_stick = wpilib.Joystick(0)
self.drivetrain = swerve.SwerveDrive(constants.chassis_length,
constants.chassis_width,
constants.swerve_config)
self.lift = lift.RD4BLift(constants.lift_ids['left'],
constants.lift_ids['right'])
self.winch = winch.Winch(constants.winch_id)
# self.claw = lift.Claw(
# constants.claw_id
# )
self.imu = IMU(wpilib.SPI.Port.kMXP)
def __init__(self):
self.gps = GPS(enabled=False)
self.imu = IMU(enabled=False)
self.steering = Steering(enabled=False)
self.brakes = Brakes()
super(BrakeTest, self).__init__(self.gps,
self.imu,
self.steering,
self.brakes,
log_data=False,
debug_prints=True)
def __init__(self):
self.gps = GPS()
self.imu = IMU()
self.lidar = LidarTurret()
self.steering = Steering()
self.brakes = Brakes()
self.underglow = Underglow()
super(FilePrinter, self).__init__(None, "rolls/2017_Feb_24", self.gps,
self.imu, self.lidar, self.steering,
self.brakes, self.underglow)
def robotInit(self):
constants.load_control_config()
wpilib.CameraServer.launch('driver_vision.py:main')
self.autoPositionSelect = wpilib.SendableChooser()
self.autoPositionSelect.addDefault('Middle-Baseline',
'Middle-Baseline')
self.autoPositionSelect.addObject('Middle-Placement',
'Middle-Placement') # noqa: E501
self.autoPositionSelect.addObject('Left', 'Left')
self.autoPositionSelect.addObject('Right', 'Right')
wpilib.SmartDashboard.putData('Robot Starting Position',
self.autoPositionSelect)
self.drivetrain = swerve.SwerveDrive(constants.chassis_length,
constants.chassis_width,
constants.swerve_config)
self.drivetrain.load_config_values()
self.lift = lift.ManualControlLift(constants.lift_ids['left'],
constants.lift_ids['right'],
constants.lift_limit_channel,
constants.start_limit_channel)
self.winch = winch.Winch(constants.winch_id)
self.throttle = wpilib.Joystick(1)
self.claw = lift.Claw(constants.claw_id, constants.claw_follower_id)
self.imu = IMU(wpilib.SPI.Port.kMXP)
self.sd_update_timer = wpilib.Timer()
self.sd_update_timer.reset()
self.sd_update_timer.start()
def __init__(self,
enable_plotting,
map_set_name,
initial_compass=None,
animate=True,
enable_cameras=True,
use_log_file_maps=True,
show_cameras=None,
day_mode=False,
only_cameras=False,
enable_pid=True,
enable_kalman=True):
# ----- initialize robot objects -----
self.gps = GPS()
self.imu = IMU()
self.steering = Steering()
self.brakes = Brakes()
self.underglow = Underglow()
super(RoboQuasar, self).__init__(
self.gps,
self.imu,
self.steering,
self.brakes,
self.underglow,
)
self.manual_mode = True
# ----- init CV classes ------
if show_cameras is None:
show_cameras = enable_plotting
self.only_cameras = only_cameras
if self.only_cameras:
self.enable_cameras = True
self.camera = Camera("rightcam",
enabled=enable_cameras,
show=show_cameras)
self.day_mode = day_mode
self.pipeline = Pipeline(self.camera,
self.day_mode,
enabled=False,
separate_read_thread=False)
# ----- init filters and controllers -----
# position state message (in or out of map)
self.position_state = ""
self.prev_pos_state = self.position_state
# init controller
self.map_set_name = map_set_name
checkpoint_map_name, inner_map_name, outer_map_name, map_dir = map_sets[
map_set_name]
self.init_checkpoints = [
0, # 0, hill 1
17, # 1, hill 2
121, # 2, hill 3
127, # 3, hill 4
143, # 4, hill 5
]
self.init_offset = 12
self.map_manipulator = MapManipulator(
checkpoint_map_name,
map_dir,
inner_map_name,
outer_map_name,
offset=2,
init_indices=self.init_checkpoints)
self.pid = None
self.enable_pid = enable_pid
self.gps_disabled = False
self.prev_gps_disabled = False
self.disabled_gps_checkpoints = [
(0, 12),
# (13, 17),
(130, len(self.map_manipulator) - 1)
]
self.angle_filter = AngleManipulator(initial_compass)
self.use_log_file_maps = use_log_file_maps
# ----- filtered outputs -----
self.imu_heading_guess = 0.0
self.steering_angle = 0.0
self.map_heading = 0.0
self.gps_corrected_lat = 0.0
self.gps_corrected_long = 0.0
self.left_limit = None
self.map_lat = 0.0
self.map_long = 0.0
self.kalman = None
self.gps_t0 = 0.0
self.imu_t0 = 0.0
# ----- dashboard (parameters to tweak) -----
self.map_weight = 0.15
self.kp = 0.9
self.kd = 0.0
self.ki = 0.0
self.angle_threshold = math.radians(10)
self.percent_threshold = 0.4
self.enable_drift_correction = False
self.enable_kalman = enable_kalman
# ----- drift correction -----
self.lat_drift = 0.0
self.long_drift = 0.0
self.drift_corrected = False
self.lat_drift_data = []
self.long_drift_data = []
self.bearing_data = []
# ----- status variables -----
self.gps_within_map = True
self.prev_within_map_state = True
self.prev_gps_state = False
self.uncertainty_condition = False
self.prev_pipeline_value_state = False
self.prev_index = -1
# ----- link callbacks -----
self.link_object(self.gps, self.receive_gps)
self.link_object(self.imu, self.receive_imu)
self.link_reoccuring(0.008, self.steering_event)
self.link_reoccuring(0.01, self.brake_ping)
# ----- init plots -----
self.quasar_plotter = RoboQuasarPlotter(
animate, enable_plotting, self.enable_kalman,
self.map_manipulator.map, self.map_manipulator.inner_map,
self.map_manipulator.outer_map, self.key_press, self.map_set_name,
False)
class Robot(wpilib.IterativeRobot):
def robotInit(self):
constants.load_control_config()
wpilib.CameraServer.launch('driver_vision.py:main')
self.autoPositionSelect = wpilib.SendableChooser()
self.autoPositionSelect.addDefault('Middle', 'Middle')
self.autoPositionSelect.addObject('Left', 'Left')
self.autoPositionSelect.addObject('Right', 'Right')
wpilib.SmartDashboard.putData('Robot Starting Position',
self.autoPositionSelect)
self.control_stick = wpilib.Joystick(0)
self.drivetrain = swerve.SwerveDrive(constants.chassis_length,
constants.chassis_width,
constants.swerve_config)
self.lift = lift.RD4BLift(constants.lift_ids['left'],
constants.lift_ids['right'])
self.winch = winch.Winch(constants.winch_id)
# self.claw = lift.Claw(
# constants.claw_id
# )
self.imu = IMU(wpilib.SPI.Port.kMXP)
def disabledInit(self):
# We don't really _need_ to reload configuration in
# every init call-- it's just useful for debugging.
# (no need to restart robot code just to load new values)
self.drivetrain.load_config_values()
def disabledPeriodic(self):
self.drivetrain.update_smart_dashboard()
self.imu.update_smart_dashboard()
def autonomousInit(self):
self.drivetrain.load_config_values()
self.auto = Autonomous(self, self.autoPositionSelect.getSelected())
self.auto.periodic()
def autonomousPeriodic(self):
self.auto.update_smart_dashboard()
self.imu.update_smart_dashboard()
self.drivetrain.update_smart_dashboard()
self.auto.periodic()
def teleopInit(self):
self.teleop = Teleop(self, self.control_stick)
self.drivetrain.load_config_values()
constants.load_control_config()
def teleopPeriodic(self):
# For now: basic driving
constants.load_control_config()
self.teleop.drive()
self.teleop.buttons()
self.teleop.lift_control()
self.drivetrain.update_smart_dashboard()
self.teleop.update_smart_dashboard()
self.imu.update_smart_dashboard()
def __init__(self, set_num=-1):
self.gps = GPS()
self.imu = IMU()
self.turret = LidarTurret()
self.steering = Steering()
self.brakes = Brakes()
self.underglow = Underglow()
joystick = None
if only_lidar:
self.gps.enabled = False
self.imu.enabled = False
self.steering.enabled = False
self.brakes.enabled = False
self.underglow.enabled = False
elif not simulated:
joystick = WiiUJoystick()
if args.includelidar:
self.turret.enabled = False
# self.imu_plot = RobotPlot("Magnetometer data (x, z vs. time)", flat_plot=True, skip_count=20,
# plot_enabled=False)
self.kalman_plots = KalmanPlots(not only_lidar, use_filter, animate)
if animate:
if args.includelidar:
self.turret.point_cloud_plot.enabled = False
self.live_plot = LivePlotter(2,
self.kalman_plots.filter_comparison,
self.turret.point_cloud_plot,
skip_count=10)
if self.turret.point_cloud_plot.enabled:
self.live_plot.draw_dot(self.turret.point_cloud_plot,
0,
0,
color='orange',
markersize=5)
elif plots_enabled:
self.static_plot = StaticPlotter(
1, self.kalman_plots.filter_comparison)
self.filter = None
# m = np.array(MapFile("buggy course checkpoints").map)
# self.map_plot.update(m[:, 0], m[:, 1])
file_sets = (
("16;49", "2017_Feb_17"), # default, latest
# data day 4
# ("15", "data_days/2017_Feb_14"), # filter explodes, LIDAR interfered by the sun
# ("16;20", "data_days/2017_Feb_14"), # shorten run, LIDAR collapsed
# ("16;57", "data_days/2017_Feb_14"), # interfered LIDAR
# ("17;10", "data_days/2017_Feb_14"), # all data is fine, interfered LIDAR
# ("17;33", "data_days/2017_Feb_14"), # data is fine, normal run
# data day 3
# ("16;38", "data_days/2017_Feb_08"),
# ("17", "data_days/2017_Feb_08"),
# ("18", "data_days/2017_Feb_08"),
# trackfield no gyro values
# ("15;46", "old_data/2016_Dec_02"),
# ("15;54", "old_data/2016_Dec_02"),
# ("16;10", "old_data/2016_Dec_02"),
# ("16;10", "old_data/2016_Dec_02")
# rolls day 1
# ("07;22", "old_data/2016_Nov_06"), # bad gyro values
# rolls day 2
# ("07;36;03 m", "old_data/2016_Nov_12"),
# ("09;12", "old_data/2016_Nov_12"), # invalid values
# ("07;04;57", "old_data/2016_Nov_13"), # bad gyro values
# rolls day 3
# ("modified 07;04", "old_data/2016_Nov_13"),
# ("modified 07;23", "old_data/2016_Nov_13"), # wonky value for mag.
# rolling on the cut
# ("16;29", "old_data/2016_Dec_09"),
# ("16;49", "old_data/2016_Dec_09"),
# ("16;5", "old_data/2016_Dec_09"),
# ("17;", "old_data/2016_Dec_09"), # nothing wrong, really short
# bad data
# ("16;07", "old_data/2016_Dec_09/bad_data"), # nothing wrong, really short
# ("16;09", "old_data/2016_Dec_09/bad_data"), # nothing wrong, really short
# ("18;00", "old_data/2016_Dec_09/bad_data"), # gps spazzed out
# ("18;02", "old_data/2016_Dec_09/bad_data"), # gps spazzed out
# ("18;09", "old_data/2016_Dec_09/bad_data"), # gps spazzed out
)
self.checkpoint_num = 0
self.lat1, self.long1, self.alt1 = 0, 0, 0
self.lat2, self.long2, self.alt2 = 0, 0, 0
self.num_recv_from_fix = None
self.received_fix = False
self.imu_t0 = 0
self.gps_t0 = 0
self.lidar_t0 = 0
if simulated:
super(RoboQuasar, self).__init__(
file_sets[set_num][0],
file_sets[set_num][1],
self.gps,
self.imu,
self.turret,
start_index=10000,
# end_index=2000,
)
extension_index = self.parser.full_path.rfind(".")
image_path = self.parser.full_path[:extension_index]
else:
super(RoboQuasar, self).__init__(self.imu,
self.gps,
self.steering,
self.brakes,
self.underglow,
self.turret,
joystick=joystick,
debug_prints=args.verbose,
log_data=args.log)
extension_index = self.logger.full_path.rfind(".")
image_path = self.logger.full_path[:extension_index]
self.slam = SLAM(self.turret, image_path + " post map")
self.slam_plots = SlamPlots(self.slam.map_size_pixels,
self.slam.map_size_meters,
args.computeslam)
class Robot(wpilib.IterativeRobot):
def robotInit(self):
constants.load_control_config()
wpilib.CameraServer.launch('driver_vision.py:main')
self.autoPositionSelect = wpilib.SendableChooser()
self.autoPositionSelect.addDefault('Middle-Baseline',
'Middle-Baseline')
self.autoPositionSelect.addObject('Middle-Placement',
'Middle-Placement') # noqa: E501
self.autoPositionSelect.addObject('Left', 'Left')
self.autoPositionSelect.addObject('Right', 'Right')
wpilib.SmartDashboard.putData('Robot Starting Position',
self.autoPositionSelect)
self.drivetrain = swerve.SwerveDrive(constants.chassis_length,
constants.chassis_width,
constants.swerve_config)
self.drivetrain.load_config_values()
self.lift = lift.ManualControlLift(constants.lift_ids['left'],
constants.lift_ids['right'],
constants.lift_limit_channel,
constants.start_limit_channel)
self.winch = winch.Winch(constants.winch_id)
self.throttle = wpilib.Joystick(1)
self.claw = lift.Claw(constants.claw_id, constants.claw_follower_id)
self.imu = IMU(wpilib.SPI.Port.kMXP)
self.sd_update_timer = wpilib.Timer()
self.sd_update_timer.reset()
self.sd_update_timer.start()
def disabledInit(self):
pass
def disabledPeriodic(self):
try:
self.lift.load_config_values()
self.drivetrain.load_config_values()
except: # noqa: E772
log_exception('disabled', 'when loading config')
try:
self.drivetrain.update_smart_dashboard()
self.imu.update_smart_dashboard()
self.lift.update_smart_dashboard()
self.winch.update_smart_dashboard()
wpilib.SmartDashboard.putNumber(
"Throttle Pos", self.throttle.getRawAxis(constants.liftAxis))
except: # noqa: E772
log_exception('disabled', 'when updating SmartDashboard')
try:
self.lift.checkLimitSwitch()
pass
except: # noqa: E772
log_exception('disabled', 'when checking lift limit switch')
self.drivetrain.update_smart_dashboard()
def autonomousInit(self):
try:
self.drivetrain.load_config_values()
self.lift.load_config_values()
except: # noqa: E772
log_exception('auto-init', 'when loading config')
self.autoPos = None
try:
self.autoPos = self.autoPositionSelect.getSelected()
except: # noqa: E772
self.autoPos = None
log_exception('auto-init', 'when getting robot start position')
try:
if self.autoPos is not None and self.autoPos != 'None':
self.auto = Autonomous(self, self.autoPos)
else:
log('auto-init', 'Disabling autonomous...')
except: # noqa: E772
log_exception('auto-init', 'in Autonomous constructor')
try:
self.lift.checkLimitSwitch()
pass
except: # noqa: E772
log_exception('auto-init', 'when checking lift limit switch')
def autonomousPeriodic(self):
try:
if self.sd_update_timer.hasPeriodPassed(0.5):
self.auto.update_smart_dashboard()
self.imu.update_smart_dashboard()
self.drivetrain.update_smart_dashboard()
self.lift.update_smart_dashboard()
self.winch.update_smart_dashboard()
except: # noqa: E772
log_exception('auto', 'when updating SmartDashboard')
try:
if self.autoPos is not None and self.autoPos != 'None':
self.auto.periodic()
except: # noqa: E772
# Stop everything.
self.drivetrain.immediate_stop()
self.lift.setLiftPower(0)
self.claw.set_power(0)
self.winch.stop()
log_exception('auto', 'in auto :periodic()')
try:
self.lift.checkLimitSwitch()
pass
except: # noqa: E772
log_exception('auto', 'when checking lift limit switch')
def teleopInit(self):
try:
self.teleop = Teleop(self)
except: # noqa: E772
log_exception('teleop-init', 'in Teleop constructor')
try:
self.drivetrain.load_config_values()
self.lift.load_config_values()
constants.load_control_config()
except: # noqa: E772
log_exception('teleop-init', 'when loading config')
try:
self.lift.checkLimitSwitch()
pass
except: # noqa: E772
log_exception('teleop-init', 'when checking lift limit switch')
def teleopPeriodic(self):
try:
self.teleop.drive()
except: # noqa: E772
log_exception('teleop', 'in drive control')
self.drivetrain.immediate_stop()
try:
self.teleop.buttons()
except: # noqa: E772
log_exception('teleop', 'in button handler')
try:
self.teleop.lift_control()
except: # noqa: E772
log_exception('teleop', 'in lift_control')
self.lift.setLiftPower(0)
try:
self.teleop.claw_control()
except: # noqa: E772
log_exception('teleop', 'in claw_control')
self.claw.set_power(0)
try:
self.teleop.winch_control()
except: # noqa: E772
log_exception('teleop', 'in winch_control')
self.winch.stop()
try:
self.lift.checkLimitSwitch()
pass
except: # noqa: E772
log_exception('teleop', 'in lift.checkLimitSwitch')
if self.sd_update_timer.hasPeriodPassed(0.5):
try:
constants.load_control_config()
self.drivetrain.load_config_values()
self.lift.load_config_values()
except: # noqa: E772
log_exception('teleop', 'when loading config')
try:
self.drivetrain.update_smart_dashboard()
self.teleop.update_smart_dashboard()
self.imu.update_smart_dashboard()
self.lift.update_smart_dashboard()
self.winch.update_smart_dashboard()
except: # noqa: E772
log_exception('teleop', 'when updating SmartDashboard')
def get_imu():
return IMU(IMU_ADDR, IMU_SCL, IMU_SDA, I2C_LOCK)
def __init__(self):
self.imu = IMU()
super(IMUrunner, self).__init__(self.imu,
log_data=True,
debug_prints=True)
def __init__(self):
self.imu = IMU()
super(IMUsimatulor, self).__init__(-1, -1, self.imu)