Fix angular reference.

dmx_manager/src/manager.py

@@ -42,11 +42,8 @@ def set_channel(self, channel, value):
     :return: None
     """
     channel -= 1
-    if channel > 511:
-      channel = 511
-    elif channel < 0:
-      channel = 0
-    self.dmx_frame[channel] = value
+    if channel >= 0 and channel < 512:
+      self.dmx_frame[channel] = value
 
   def get_channel(self, channel):
     """
@@ -55,11 +52,10 @@ def get_channel(self, channel):
     :return: 8 bit value for that channel
     """
     channel -= 1
-    if channel > 511:
-      channel = 511
-    elif channel < 0:
-      channel = 0
-    return self.dmx_frame[channel]
+    if channel >= 0 and channel < 512:
+      return self.dmx_frame[channel]
+    else:
+      return None
 
   def position_cb(self, data):
     x = 128 - (data.x - 320) * 20 / 320

optical_tracker/src/calib.py

@@ -3,13 +3,14 @@
 import rospy
 import pygame
 import math
+import numpy as np
 from pygame import key
 from geometry_msgs.msg import Point
 from dmx_msgs.srv import GetChannel, GetChannelRequest
 from widgets import PanGauge, TiltGauge
 
-DEGREES_PER_PAN = 540 / 255
-DEGREES_PER_TILT = 270 / 255
+DEGREES_PER_PAN = 540.0 / 255
+DEGREES_PER_TILT = 270.0 / 255
 RADIANS_PER_PAN = DEGREES_PER_PAN * math.pi / 180
 RADIANS_PER_TILT = DEGREES_PER_TILT * math.pi / 180
 
@@ -29,12 +30,17 @@ def __init__(self):
         self.get_channel = rospy.ServiceProxy("/get_channel", GetChannel)
 
     def calibrate(self):
-        pan = 0
-        tilt = 0
-        print "Calibrating... Press ESC to end."
-        inc_pan = 0.05
-        inc_tilt = 0.05
+        print "Calibrating..."
+        pass
+
+    def run(self):
+        print "\nCalibration program.\n"
+        print "Press 'c' to enter a calibration point.\n\n"
         while not rospy.is_shutdown():
+            # update gauges
+            norm_pan_rad = math.radians(self.normalize_pan(self.get_pan()))
+            norm_tilt_rad = math.radians(self.normalize_tilt(self.get_tilt()))
+            self.update_display(norm_pan_rad, norm_tilt_rad)
             pygame.event.pump()
             keys = key.get_pressed()
             # pan = pan + inc_pan
@@ -55,15 +61,6 @@ def calibrate(self):
                 return
             self.rate.sleep()
 
-    def run(self):
-        #print "Press C to start calibration."
-        #while not rospy.is_shutdown():
-        #    pygame.event.pump()
-        #    keys = key.get_pressed()
-        #    if keys[pygame.K_c]:
-        self.calibrate()
-        #    self.rate.sleep()
-
     def position_cb(self, point):
         assert(isinstance(point, Point))
         self.latest_point = point
@@ -72,9 +69,6 @@ def get_normalized_pan(self):
         pan = self.get_channel(1).value * DEGREES_PER_PAN + 180
         return pan * math.pi / 180
 
-    def get_normalized_tilt(self):
-        tilt = -self.get_channel(3).value * DEGREES_PER_TILT + 45 + 90
-        return tilt * math.pi / 180
 
 if __name__ == '__main__':
   rospy.init_node("test_calibration")

optical_tracker/src/widgets.py

@@ -24,7 +24,7 @@ def draw(self, surface, x=0, y=0):
         color = self.colors['marker']
         start = [x + self.width / 2, y + self.height]
         radius = self.width / 2
-        end = [start[0] + math.cos(self.angle) * radius, start[1] - math.sin(self.angle) * radius]
+        end = [start[0] - math.cos(self.angle + math.pi/2) * radius, start[1] - math.sin(self.angle + math.pi/2) * radius]
         pygame.draw.line(surface, color, start, end, 2)
 
     def draw_border(self, surface, x, y):
@@ -58,11 +58,11 @@ def set_tilt(self, tilt):
     def draw(self, surface, x=0, y=0):
         # type: (pygame.Surface, int, int) -> None
         self.draw_border(surface, x, y)
-        self.draw_text(surface, x + 5, y + 5, 'Pan: %d' % math.ceil(math.degrees(self.angle)))
+        self.draw_text(surface, x + 5, y + 5, 'Tilt: %d' % math.ceil(math.degrees(self.angle)))
         color = self.colors['marker']
         start = [x + self.width, y + self.height / 2]
         radius = self.height / 2
-        end = [start[0] + math.cos(self.angle + math.pi/2) * radius, start[1] - math.sin(self.angle + math.pi/2) * radius]
+        end = [start[0] + math.cos(self.angle + math.pi) * radius, start[1] + math.sin(self.angle + math.pi) * radius]
         pygame.draw.line(surface, color, start, end, 2)
 
     def draw_border(self, surface, x, y):