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)