diff --git a/robot/camera/image_processor.py b/robot/camera/image_processor.py index 6efe9f3..6f431b1 100755 --- a/robot/camera/image_processor.py +++ b/robot/camera/image_processor.py @@ -5,6 +5,8 @@ from networktables import NetworkTable from networktables.util import ntproperty +INF = float('inf') + class ImageProcessor: # Values for the lifecam-3000 @@ -39,6 +41,9 @@ class ImageProcessor: draw_gear_patch = ntproperty('/camera/processor/draw_gear_patch', False) draw_gear_target = ntproperty('/camera/processor/draw_gear_target', True) + # found, time, angle + target = ntproperty('/camera/target', [0.0, 0.0, INF, INF]) + def __init__(self): self.size = None self.thresh_low = np.array([self.thresh_hue_lower, self.thresh_sat_lower, self.thresh_val_lower], dtype=np.uint8) @@ -169,6 +174,8 @@ def process_for_gear_target(self, contours, time): # Breaks out of loop if no complete targets if len(self.full_targets) == 0: + self.target = (0, time, INF, INF) + self.nt.putBoolean('gear_target_present', False) return self.out @@ -251,9 +258,8 @@ def process_for_gear_target(self, contours, time): #print("Skew %s" % skew) self.nt.putNumber('gear_target_skew', skew) - target = (angle, skew, time) + self.target = (1, time, angle, skew) - self.nt.putNumberArray('target', target) self.nt.putNumber('gear_target_angle', angle) self.nt.putNumber('gear_target_height', height) diff --git a/robot/controllers/auto_align.py b/robot/controllers/auto_align.py index c0e2f2d..c4b4c3b 100644 --- a/robot/controllers/auto_align.py +++ b/robot/controllers/auto_align.py @@ -1,8 +1,5 @@ -import hal -import wpilib -from magicbot import state, timed_state, StateMachine, tunable -from networktables import NetworkTable +from magicbot import state, StateMachine, tunable from networktables.util import ntproperty from components.swervedrive import SwerveDrive @@ -24,64 +21,43 @@ class AutoAlign(StateMachine): ideal_skew = tunable(-0.967) ideal_angle = tunable(-1.804) + # found, time, angle, skew + target = ntproperty('/camera/target', (0.0, 0.0, float('inf'), float('inf'))) + def __init__(self): - target = None - - nt = NetworkTable.getTable('/camera') - nt.addTableListener(self._on_target, True, 'target') - + self.lasttime = 0 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(captur_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 moving_to_position(self, initial_call): - @state(first = True) - def inital_state(self): - - self.target = None - self.aimed_at_angle = None + if initial_call: + self.aimed_at_angle = None + self.pos_history.enable() + self.cv_enabled = True - self.pos_history.enable() + found, time, offset, skew = self.target - self.cv_enabled = True + # do I have new information? + if self.lasttime < time: + history = self.pos_history.get_position(time) + + if found > 0 and history is not None: + r_angle, r_x, r_y, r_time = history + + self.aimed_at_angle = r_angle + offset - self.ideal_angle - self.next_state('moving_to_position') + self.lasttime = time - @state - def moving_to_position(self): + if self.aimed_at_angle is not None: + self.angle_ctrl.align_to(self.aimed_at_angle) - if self._move_to_position(): - self.next_state('done') - - @state - def end(self): - pass + #return self.angle_ctrl.is_aligned() def done(self): super().done() diff --git a/robot/controllers/position_history.py b/robot/controllers/position_history.py index ae8c38c..477417b 100644 --- a/robot/controllers/position_history.py +++ b/robot/controllers/position_history.py @@ -5,13 +5,15 @@ import threading from controllers.angle_controller import AngleController -from components import swervedrive +from controllers.pos_controller import XPosController, YPosController from collections import deque class PositionHistory: angle_ctrl = AngleController + y_ctrl = YPosController + x_ctrl = XPosController def __init__(self): diff --git a/robot/physics.py b/robot/physics.py index c9ba6e8..b49f52e 100644 --- a/robot/physics.py +++ b/robot/physics.py @@ -4,6 +4,11 @@ import wpilib import ctre +try: + from pyfrc.physics.visionsim import VisionSim +except ImportError: + VisionSim = None + class PhysicsEngine: @@ -11,11 +16,29 @@ class PhysicsEngine: lr_degrees = ntproperty("/SmartDashboard/drive/rl_module/degrees", 0) rf_degrees = ntproperty("/SmartDashboard/drive/fr_module/degrees", 0) lf_degrees = ntproperty("/SmartDashboard/drive/fl_module/degrees", 0) + + target = ntproperty('/camera/target', [0.0, 0.0, float('inf')]) def __init__(self, controller): self.controller = controller self.controller.add_device_gyro_channel('navxmxp_spi_4_angle') + + if VisionSim is not None: + targets = [ + # right + VisionSim.Target(16, 12, 250, 20), # angle is 122.23 + # middle + VisionSim.Target(18.5, 16, 295, 65), # angle is 180 + # left + VisionSim.Target(16, 20, 340, 110), # angle is -142 + ] + + self.vision = VisionSim(targets, 61.0, + 1.5, 15, 15, + physics_controller=controller) + else: + self.vision = None @@ -78,6 +101,15 @@ def update_sim(self, hal_data, now, tm_diff): self.controller.vector_drive(vx, vy, vw, tm_diff) except: pass + + if self.vision: + x, y, angle = self.controller.get_position() + + data = self.vision.compute(now, x, y, angle) + if data is not None: + # found, time, angle, skew + data = data[0] + self.target = (data[0], data[1], data[2], float('inf')) #data[0][:3] diff --git a/robot/robot.py b/robot/robot.py index 88069ee..23d3a88 100755 --- a/robot/robot.py +++ b/robot/robot.py @@ -25,7 +25,6 @@ class MyRobot(magicbot.MagicRobot): - drive = swervedrive.SwerveDrive shooter = shooter.Shooter gear_picker = gearpicker.GearPicker climber = climber.Climber @@ -42,11 +41,13 @@ class MyRobot(magicbot.MagicRobot): fc_y_ctrl = FCYPosController fc_x_ctrl = FCXPosController + auto_align = AutoAlign + angle_ctrl = AngleController moving_angle_ctrl = MovingAngleController pos_history = PositionHistory - auto_align = AutoAlign + drive = swervedrive.SwerveDrive gamepad_mode = tunable(False) diff --git a/robot/sim/config.json b/robot/sim/config.json index e1d7890..eb48454 100644 --- a/robot/sim/config.json +++ b/robot/sim/config.json @@ -4,8 +4,8 @@ "w": 3.6, "h": 3, "starting_x": 25, - "starting_y": 25, - "starting_angle": 180 + "starting_y": 23, + "starting_angle": 180 }, "analog": { "0": "Rear Right Rotation Encoder", @@ -42,7 +42,7 @@ "field": { "w": 27, "h": 27, - "px_per_ft": 10 + "px_per_ft": 17 } } } \ No newline at end of file