Vision sim

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)

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()

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):
 

robot/physics.py

@@ -4,18 +4,41 @@
 import wpilib
 import ctre
 
+try:
+    from pyfrc.physics.visionsim import VisionSim
+except ImportError:
+    VisionSim = None
+
 
 class PhysicsEngine:
 
     rr_degrees = ntproperty("/SmartDashboard/drive/rr_module/degrees", 0)
     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]
 
 
 

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)
 

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
 		}
 	}
 }