1 files changed, 26 insertions, 19 deletions
diff --git a/pybot/drivers/motors.py b/pybot/drivers/motors.py
index 02421d9..e62192a 100644
--- a/pybot/drivers/motors.py
+++ b/pybot/drivers/motors.py
@@ -2,26 +2,33 @@
 RD_VERSION = 0x00
 SET_VEL_2MTR = 0x01
 SET_VEL_MTR = 0x02
+TEST_MOTORS = 0x03
+GET_TYPE = 0x04
 
-f1 = {
-    'name': 'getVersion',
-    'call': RD_VERSION,
-    'params': 0,
-    'read': 3
-}
+def getVersion(dev):
+    dev.send([RD_VERSION])
+    raw = dev.read(3)
+    return raw[1] + raw[2] * 256
 
-f2 = {
-    'name': 'setvel2mtr',
-    'call': SET_VEL_2MTR,
-    'params': 6,
-    'read': 1
-}
+def setvel2mtr(dev, sentido1, vel1, sentido2, vel2):
+    msg = [SET_VEL_2MTR, sentido1, vel1 / 256, vel1 % 256, sentido2, vel2 / 256, vel2 % 256]
+    dev.send(msg)
+    raw = dev.read(1)
+    return raw[0]
 
-f3 = {
-    'name': 'setvelmtr',
-    'call': SET_VEL_MTR,
-    'params': 4,
-    'read': 1
-}
+def setvelmtr(dev, motor_id, sentido, vel):
+    msg = [SET_VEL_MTR, motor_id, sentido, vel / 256, vel % 256]
+    dev.send(msg)
+    raw = dev.read(1)
+    return raw[0]
+
+def testMotors(dev):
+    dev.send([TEST_MOTORS])
+    raw = dev.read(1)
+    return raw[0]
+
+def getType(dev):
+    dev.send([GET_TYPE])
+    raw = dev.read(2)
+    return raw[1]
 
-FUNCTIONS = [f1, f2, f3]