From 798fec42bfdbef703121caa99b7d9572d4feadc3 Mon Sep 17 00:00:00 2001 From: Dustin Spicuzza Date: Tue, 7 Mar 2017 01:57:27 -0500 Subject: [PATCH 1/3] Add pyfrc camera simulation support - For some reason the targets don't work correctly --- robot/physics.py | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/robot/physics.py b/robot/physics.py index c9ba6e8..7f0a6ff 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,28 @@ 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) + else: + self.vision = None @@ -78,6 +100,13 @@ 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: + self.target = data[0][:3] From 604b93f1701a900c256196ca7938f2f03a0d2ad7 Mon Sep 17 00:00:00 2001 From: Dustin Spicuzza Date: Tue, 7 Mar 2017 02:02:51 -0500 Subject: [PATCH 2/3] Change px per feet and starting position --- robot/sim/config.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robot/sim/config.json b/robot/sim/config.json index e1d7890..52f8dbe 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": 16, + "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 From 85629160fc901ec6524c365b36ee3003a283fd35 Mon Sep 17 00:00:00 2001 From: Dustin Spicuzza Date: Fri, 7 Apr 2017 15:33:46 -0400 Subject: [PATCH 3/3] Auto align works in the sim --- robot/camera/image_processor.py | 10 +++- robot/controllers/auto_align.py | 74 +++++++++------------------ robot/controllers/position_history.py | 4 +- robot/physics.py | 7 ++- robot/robot.py | 5 +- robot/sim/config.json | 2 +- 6 files changed, 45 insertions(+), 57 deletions(-) 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 7f0a6ff..b49f52e 100644 --- a/robot/physics.py +++ b/robot/physics.py @@ -35,7 +35,8 @@ def __init__(self, controller): ] self.vision = VisionSim(targets, 61.0, - 1.5, 15, 15) + 1.5, 15, 15, + physics_controller=controller) else: self.vision = None @@ -106,7 +107,9 @@ def update_sim(self, hal_data, now, tm_diff): data = self.vision.compute(now, x, y, angle) if data is not None: - self.target = data[0][:3] + # 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 52f8dbe..eb48454 100644 --- a/robot/sim/config.json +++ b/robot/sim/config.json @@ -4,7 +4,7 @@ "w": 3.6, "h": 3, "starting_x": 25, - "starting_y": 16, + "starting_y": 23, "starting_angle": 180 }, "analog": {