Merge pull request #4 from LiamBindle/get-values

Get Values from VESC sensors and encoder

pyvesc/examples/get_values.py

@@ -0,0 +1,48 @@
+import pyvesc
+from pyvesc import GetValues, SetRPM, SetCurrent, SetRotorPositionMode, GetRotorPosition
+import serial
+import time
+
+# Set your serial port here (either /dev/ttyX or COMX)
+serialport = 'COM3'
+
+def get_values_example():
+    with serial.Serial(serialport, baudrate=115200, timeout=0.05) as ser:
+        try:
+            # Optional: Turn on rotor position reading if an encoder is installed
+            ser.write(pyvesc.encode(SetRotorPositionMode(SetRotorPositionMode.DISP_POS_OFF)))
+            while True:
+                # Set the ERPM of the VESC motor
+                #    Note: if you want to set the real RPM you can set a scalar
+                #          manually in setters.py
+                #          12 poles and 19:1 gearbox would have a scalar of 1/228
+                ser.write(pyvesc.encode(SetRPM(10000)))
+
+                # Request the current measurement from the vesc
+                ser.write(pyvesc.encode_request(GetValues))
+
+                # Check if there is enough data back for a measurement
+                if ser.in_waiting > 61:
+                    (response, consumed) = pyvesc.decode(ser.read(61))
+
+                    # Print out the values
+                    try:
+                        print(response.rpm)
+
+                    except:
+                        # ToDo: Figure out how to isolate rotor position and other sensor data
+                        #       in the incoming datastream
+                        #try:
+                        #    print(response.rotor_pos)
+                        #except:
+                        #    pass
+                        pass
+
+                time.sleep(0.1)
+
+        except KeyboardInterrupt:
+            # Turn Off the VESC
+            ser.write(pyvesc.encode(SetCurrent(0)))
+
+if __name__ == "__main__":
+    get_values_example()

pyvesc/interface.py

@@ -26,7 +26,7 @@ def encode(msg):
 
     :param msg: Message to be encoded. All fields must be initialized.
     :type msg: PyVESC message
-    
+
     :return: The packet.
     :rtype: bytes
     """
@@ -35,7 +35,7 @@ def encode(msg):
     return packet
 
 
-def encode_getter_request(msg_cls):
+def encode_request(msg_cls):
     """
     Encodes a PyVESC message for requesting a getter message. This function
     should be called when you want to request a VESC to return a getter
@@ -47,6 +47,6 @@ def encode_getter_request(msg_cls):
     :return: The encoded PyVESC message which can be sent.
     :rtype: bytes
     """
-    msg_payload = pyvesc.messages.base.VESCMessage.pack(msg_cls, True)
+    msg_payload = pyvesc.messages.base.VESCMessage.pack(msg_cls, header_only=True)
     packet = pyvesc.packet.codec.frame(msg_payload)
-    return packet
+    return packet

pyvesc/messages/base.py

@@ -7,6 +7,7 @@ class VESCMessage(type):
     This is the metaclass for any VESC message classes. A VESC message class must then declare 2 static attributes:
     id: unsigned integer which is the identification number for messages of this class
     fields: list of tuples. tuples are of size 2, first element is the field name, second element is the fields type
+            the third optional element is a scalar that will be applied to the data upon unpack
     format character. For more info on struct format characters see: https://docs.python.org/2/library/struct.html
     """
     _msg_registry = {}
@@ -29,8 +30,13 @@ def __init__(cls, name, bases, clsdict):
         cls._string_field = None
         cls._fmt_fields = ''
         cls._field_names = []
+        cls._field_scalars = []
         for field, idx in zip(cls.fields, range(0, len(cls.fields))):
             cls._field_names.append(field[0])
+            try:
+                cls._field_scalars.append(field[2])
+            except IndexError:
+                pass
             if field[1] is 's':
                 # string field, add % so we can vary the length
                 cls._fmt_fields += '%u'
@@ -70,7 +76,12 @@ def unpack(msg_bytes):
             fmt_w_string = msg_type._fmt_fields % (len_string)
             data = struct.unpack_from(VESCMessage._endian_fmt + fmt_w_string, msg_bytes, 1)
         else:
-            data = struct.unpack_from(VESCMessage._endian_fmt + msg_type._fmt_fields, msg_bytes, 1)
+            data = list(struct.unpack_from(VESCMessage._endian_fmt + msg_type._fmt_fields, msg_bytes, 1))
+            for k, field in enumerate(data):
+                try:
+                    data[k] = data[k]/msg_type._field_scalars[k]
+                except (TypeError, IndexError) as e:
+                    pass
         msg = msg_type(*data)
         if not (msg_type._string_field is None):
             string_field_name = msg_type._field_names[msg_type._string_field]
@@ -85,8 +96,18 @@ def pack(instance, header_only = None):
             return struct.pack(VESCMessage._endian_fmt + VESCMessage._id_fmt, instance.id)
 
         field_values = []
-        for field_name in instance._field_names:
-            field_values.append(getattr(instance, field_name))
+        #for field_name,field_scalar in zip(instance._field_names, instance._field_scalars):
+            #print(field_name, field_scalar)
+        #if instance._field_scalars:
+        #    for field_name, field_scalar in zip(instance._field_names, instance._field_scalars):
+        #        field_values.append(getattr(instance, field_name*field_scalar))
+        #else:
+        if not instance._field_scalars:
+            for field_name in instance._field_names:
+                field_values.append(getattr(instance, field_name))
+        else:
+            for field_name, field_scalar in zip(instance._field_names, instance._field_scalars):
+                field_values.append(int(getattr(instance, field_name) * field_scalar))
         if not (instance._string_field is None):
             # string field
             string_field_name = instance._field_names[instance._string_field]
@@ -97,4 +118,3 @@ def pack(instance, header_only = None):
             return struct.pack(fmt, *values)
         else:
             return struct.pack(VESCMessage._endian_fmt + VESCMessage._id_fmt + instance._fmt_fields, *((instance.id,) + tuple(field_values)))
-

pyvesc/messages/getters.py

@@ -0,0 +1,42 @@
+from pyvesc.messages.base import VESCMessage
+
+class GetValues(metaclass=VESCMessage):
+    """
+    Gets internal sensor data
+    """
+    id = 4
+
+    fields = [
+            ('temp_mos1', 'h', 10),
+            ('temp_mos2', 'h', 10),
+            ('temp_mos3', 'h', 10),
+            ('temp_mos4', 'h', 10),
+            ('temp_mos5', 'h', 10),
+            ('temp_mos6', 'h', 10),
+            ('temp_pcb',  'h', 10),
+            ('current_motor', 'i', 100),
+            ('current_in',  'i', 100),
+            ('duty_now',    'h', 1000),
+            ('rpm',         'i', 1),
+            ('v_in',        'h', 10),
+            ('amp_hours',   'i', 10000),
+            ('amp_hours_charged', 'i', 10000),
+            ('watt_hours',  'i', 10000),
+            ('watt_hours_charged', 'i', 10000),
+            ('tachometer', 'i', 1),
+            ('tachometer_abs', 'i', 1),
+            ('mc_fault_code', 'c')
+    ]
+
+
+class GetRotorPosition(metaclass=VESCMessage):
+    """
+    Gets rotor position data
+        Note: Must be set to DISP_POS_MODE_ENCODER or DISP_POS_MODE_PID_POS (Mode 3 or Mode 4)
+        This is set by SetRotorPositionMode (id=21)
+    """
+    id = 21
+
+    fields = [
+            ('rotor_pos', 'i', 100000)
+    ]

pyvesc/messages/setters.py

@@ -43,3 +43,34 @@ class SetCurrentBrake(metaclass=VESCMessage):
     fields = [
         ('current_brake', 'i')
     ]
+
+class SetPosition(metaclass=VESCMessage):
+    """
+        Set the rotor angle based off of an encoder or sensor
+        :ivar pos: Value to set the current position or angle to.
+    """
+    id = 9
+    fields = [
+        ('pos', 'i', 1000000)
+    ]
+
+class SetRotorPositionMode(metaclass=VESCMessage):
+    """
+        Sets the rotor position feedback mode.
+        It is reccomended to use the defined modes as below
+            DISP_POS_OFF
+            DISP_POS_MODE_ENCODER
+            DISP_POS_MODE_PID_POS
+            DISP_POS_MODE_PID_POS_ERROR
+        :ivar pos_mode: Value of the mode
+    """
+
+    DISP_POS_OFF = 0
+    DISP_POS_MODE_ENCODER = 3
+    DISP_POS_MODE_PID_POS = 4
+    DISP_POS_MODE_PID_POS_ERROR = 5
+
+    id = 10
+    fields = [
+        ('pos_mode', 'b')
+    ]

test.py

@@ -242,7 +242,7 @@ def test_multiple_messages(self):
         from pyvesc.messages.base import VESCMessage
 
         class testMsg1(metaclass=VESCMessage):
-            id = 0x15
+            id = 0x45
             fields = [
                 ('f1', 'B'),
                 ('f2', 'H'),
@@ -381,7 +381,7 @@ def test_interface(self):
         from pyvesc.messages import VESCMessage
 
         class testMsg1(metaclass=VESCMessage):
-            id = 0x15
+            id = 0x45
             fields = [
                 ('f1', 'B'),
                 ('f2', 'H'),