Changeset 747 for trunk

Timestamp:

06/06/10 14:31:39 (20 months ago)

Author:

mosttw

Message:

CAN debugging code from yesterday

Files:

• trunk/software/rb/can.py (modified) (10 diffs)

trunk/software/rb/can.py

@@ -1 @@
-#!/usr/bin/python
+#!/usr/bin/python -u
 # Copyright (C) 2010 Thomas W. Most
 #
@@ -64 +64 @@
         escaped_bytes = jaguar_escape(bytes)
         message = '\xff%c%s' % (len(bytes), bytes)
+        print "writing", hex_bytes(message)
         try:
             self.serial.write(message)
@@ -71 +72 @@
             raise IOError('CAN error')
 
-    #def receive(self):
-    #    '''
-    #    Receive a message from the can bus.  Return a two-tuple of
-    #    (can_id, bytes) if a valid message is received, or raises
-    #    IOError if an invalid message is received.
-    #    '''
-    #    message = ''
-    #    while True:
-    #        
+    def recieve(self):
+        '''
+        Receive a message from the can bus.  Return a two-tuple of
+        (can_id, bytes) if a valid message is received, or raises
+        IOError if an invalid message is received.
+        '''
+        message = ''
+        while self.serial.inWaiting():
+            message += self.serial.read(self.serial.inWaiting())
+        print "read", hex_bytes(message)
+        return message
+
+    def send_heartbeat(self):
+        # Broadcast message on the system heartbeat API
+        can_id = build_can_id(
+            device_type=DEVICE_TYPE_BROADCAST,
+            manufacturer=MANF_BROADCAST,
+            api_class=0,
+            api_index=SYS_API_HEARTBEAT,
+            #api_index=0,
+            device_number=0
+        )
+        self.send(can_id)
+
+def hex_bytes(bytes):
+    return ' '.join('{0:02x}'.format(ord(b)) for b in bytes)
 
 def jaguar_escape(plain_bytes):
@@ -111 +129 @@
         api_class << 10 |
         api_index << 6 |
-        device_number
+        device_number << 0
     )
 
@@ -174 +192 @@
         assert can_bus is not None
         self.can_bus = can_bus
-        self.left_controller_nums = [2, 3, 6]
+        self.left_controller_nums = [2, 3]
         self.right_controller_nums = [4, 5]
         self.controller_nums = (self.left_controller_nums +
                                 self.right_controller_nums)
         if mode == 'speed':
-            self.init_speed_control()
-
-    def init_speed_control(self):
+            self.init_speed_control(p=0.35, i=0.003, d=0.001)
+
+    def init_speed_control(self, p, i, d):
         # Configure encoder lines
         for num in self.controller_nums:
@@ -198 +216 @@
                 device_number=num
             )
-            self.can_bus.send(id, float_to_fixed1616(0.35))
+            self.can_bus.send(id, float_to_fixed1616(p))
         for num in self.controller_nums:
             id = build_can_id(
@@ -205 +223 @@
                 device_number=num
             )
-            self.can_bus.send(id, float_to_fixed1616(0.003))
+            self.can_bus.send(id, float_to_fixed1616(i))
         for num in self.controller_nums:
             id = build_can_id(
@@ -212 +230 @@
                 device_number=num
             )
-            self.can_bus.send(id, float_to_fixed1616(0.001))
+            self.can_bus.send(id, float_to_fixed1616(d))
     
         # Enable speed control
@@ -294 +312 @@
             api_class=API_VOLTAGE_CONTROL,
             api_index=VOLTAGE_SET,
-            device_number=6)
+            device_number=3)
+    bus.send(heartbeat_can_id)
     try:
         print "Ramping up"
-        for dc in range(0, 32767, 10):
-            bus.send(heartbeat_can_id, '')
+        for dc in range(0, 32767, 100):
+            #bus.send(heartbeat_can_id)
             bus.send(set_voltage_can_id, struct.pack('<h', dc))
             time.sleep(0.05)
         print "At full speed (Ctrl-C to ramp down)"
         while True:
+            #bus.send(heartbeat_can_id)
             bus.send(set_voltage_can_id, struct.pack('<h', 32767))
             time.sleep(0.05)
     except KeyboardInterrupt:
         print "Ramping down"
-    for dc in range(32767, 0, -10):
-        bus.send(heartbeat_can_id, '')
+    for dc in range(32767, 0, -100):
+        #bus.send(heartbeat_can_id)
         bus.send(set_voltage_can_id, struct.pack('<h', dc))
         time.sleep(0.05)
 
-if __name__ == '__main__':
-    demo()
+def demo2():
     import time
     bus = CANBus('/dev/ttyS0')
@@ -333 +352 @@
         time.sleep(0.05)
     print "Duuuuh."
+
+def demo_heartbeat():
+    import time
+    bus = CANBus('/dev/ttyS0')
+    print "Sending heartbeat messages",
+    while True:
+        bus.send_heartbeat()
+        print ".",
+        time.sleep(1.0 / 150.0)
+
+def color(s, color):
+    n = dict(red=31, green=32)[color]
+    return "\x1b[{0}m{1}\x1b[0m".format(n, s)
+
+def demo_set_voltage(bus, controller_num):
+    import time
+    enable_voltage_control_can_id = build_can_id(
+            api_class=API_VOLTAGE_CONTROL,
+            api_index=VOLTAGE_MODE_ENABLE,
+            device_number=controller_num)
+    set_voltage_can_id = build_can_id(
+            api_class=API_VOLTAGE_CONTROL,
+            api_index=VOLTAGE_SET,
+            device_number=controller_num)
+    dc = 0x0800
+
+    bus.send_heartbeat()
+
+    print color("Enabling voltage control", "green")
+    bus.send(enable_voltage_control_can_id)
+    while True:
+        bus.send_heartbeat()
+        time.sleep(1.0 / 150.0)
+        if bus.recieve():
+            break
+
+    print color("Setting voltage to 0x{0:04x}".format(dc), "green")
+    bus.send(set_voltage_can_id, struct.pack('<h', dc))
+    while True:
+        bus.send_heartbeat()
+        time.sleep(1.0 / 150.0)
+        if bus.recieve():
+            break
+
+def demo_query_voltage(bus, controller_num):
+    import time
+    set_voltage_can_id = build_can_id(
+            api_class=API_VOLTAGE_CONTROL,
+            api_index=VOLTAGE_SET,
+            device_number=controller_num)
+    print color("Reading voltage", "green")
+    bus.send(set_voltage_can_id)
+    while True:
+        bus.send_heartbeat()
+        time.sleep(1.0 / 150.0)
+        if bus.recieve():
+            break
+
+if __name__ == '__main__':
+    demo_heartbeat()
+    bus = CANBus('/dev/ttyS0')
+    num = 2
+    demo_set_voltage(bus, num)
+    demo_query_voltage(bus, num)
+