diff --git a/rcb4/assets/system/99-rcb4-udev.rules b/rcb4/assets/system/99-rcb4-udev.rules index 75cd279d..20db9aa7 100644 --- a/rcb4/assets/system/99-rcb4-udev.rules +++ b/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'" diff --git a/rcb4/ics.py b/rcb4/ics.py index 1a1b2901..7267e58d 100644 --- a/rcb4/ics.py +++ b/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.