Merge pull request #135 from iory/ics

Add get current and temperature function for ics

rcb4/assets/system/99-rcb4-udev.rules

@@ -1,3 +1,3 @@
 SUBSYSTEMS=="usb", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", SYMLINK+="ftdi-$attr{serial}", MODE="0666"
 SUBSYSTEMS=="usb", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", SYMLINK+="aki-$attr{serial}", MODE="0666"
-SUBSYSTEMS=="usb", ATTRS{idVendor}=="165c", ATTRS{idProduct}=="0008", SYMLINK+="rcb4-$attr{serial}", MODE="0666"
+SUBSYSTEM=="usb", ATTR{idVendor}=="165c", ATTR{idProduct}=="0008", RUN+="/sbin/modprobe ftdi-sio", RUN+="/bin/sh -c 'echo 165C 0008 > /sys/bus/usb-serial/drivers/ftdi_sio/new_id'"

rcb4/ics.py

@@ -9,6 +9,9 @@
 import serial.tools.list_ports
 import yaml
 
+from rcb4.rcb4interface import interpolate_currents
+from rcb4.rcb4interface import interpolate_or_extrapolate_temperatures
+
 degree_to_pulse = 29.633
 
 
@@ -199,6 +202,26 @@ def set_servo_id(self, servo_id):
         ret = self.ics.read(5)
         return 0x1F & ret[4]
 
+    def get_current(self, sid=None, interpolate=True):
+        if sid is None:
+            sid = self.get_servo_id()
+        self.ics.write(bytes([0xA0 | (sid & 0x1F), 0x03]))
+        time.sleep(0.05)
+        v = self.ics.read(5)
+        if interpolate:
+            return interpolate_currents(v[4])
+        return v[4]
+
+    def get_temperature(self, sid=None, interpolate=True):
+        if sid is None:
+            sid = self.get_servo_id()
+        self.ics.write(bytes([0xA0 | (sid & 0x1F), 0x04]))
+        time.sleep(0.05)
+        v = self.ics.read(5)
+        if interpolate:
+            return interpolate_or_extrapolate_temperatures(v[4])
+        return v[4]
+
     def set_response(self, value, servo_id=None):
         """
         Set the response parameter to the specified value.