4
\$\begingroup\$

I am trying to send data from an Arduino into Python via USB every 100 ms. What improvements can be made to my code? I feel like it is kind of bloated and can be better written, but I'm not sure which improvements should be made.

read_arduino.py

import serial

def _has_digits(string):
    return any(char.isdigit() for char in str(string))

def _check_for_overflowed_digits(string):
    if _has_digits(string):
        overflowed_digits = [ char for char in string.split() if char.isdigit() ]
        digits_to_string = ("").join(overflowed_digits)
        return digits_to_string
    else:
        return None

def _compute_state_vector(data_string):
    data_array = data_string.split(",")
    state_vector = {
        'pendulum_angle': data_array[0],
        'cart_position': data_array[1],
        'pendulum_angular_velocity': data_array[2],
        # Sometimes the carriage return "\r" remains in the string, so get rid of it
        'cart_velocity': (data_array[3] if '\r' not in data_array[3] else data_array[3].split('\r')[0])
    }

    return state_vector

def _compute_input(state):
    state_vector = [state['pendulum_angle'], state['cart_position'], state['pendulum_angular_velocity'], state['cart_velocity']]
    gain_matrix = np.array([
        [1000, 0, 0, 0],
        [0, 10, 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1],
    ])

    control_input = -gain_matrix * state_vector
    return control_input

class Arduino():
    def __init__(self, port, baud_rate, timeout):
        self.serial = serial.Serial(port, baud_rate, timeout = timeout)

    def start_control(self):

        last_line_recieved = ""

        try:
            while True:
                serial_bytes = self.serial.readline()
                decoded_line = str(serial_bytes.decode('utf8'))

                # Read from serial until we capture the whole line of data
                if (len(serial_bytes) != 0) and (b'\n' in serial_bytes):

                    # Sometimes digits overflow from one line onto the next
                    overflowed_digits = _check_for_overflowed_digits(decoded_line)
                    if overflowed_digits is not None:
                        last_line_recieved += overflowed_digits

                    # Parse data into state_vector and compute control input using LQR
                    state_vector = _compute_state_vector(last_line_recieved)
                    control_input = compute_input(state_vector)
                    print(control_input)

                    # Reset the variable
                    last_line_recieved = ""

                elif len(serial_bytes) != 0:
                    last_line_recieved += decoded_line

                else:
                    pass

        except KeyboardInterrupt:
            print("You have quit reading from the serial port.")
            pass


if __name__ == "__main__":
    arduino = Arduino("/dev/cu.usbmodem14101", baud_rate = 9400, timeout = 0)
    arduino.start_control()

main.cpp

#define ENCODER_OPTIMIZE_INTERRUPTS

#include <Arduino.h>
#include <Encoder.h>
#include "motorControllerDrokL298.h"
#include "pythonUtils.h"

// Initialize encoders
#define cartEncoderPhaseA 3
#define cartEncoderPhaseB 4
#define pendulumEncoderPhaseA 2
#define pendulumEncoderPhaseB 5

Encoder cartEncoder(cartEncoderPhaseA, cartEncoderPhaseB);
Encoder pendulumEncoder(pendulumEncoderPhaseA, pendulumEncoderPhaseB);

// Initialize named constants
const unsigned long TIMEFRAME = 100; // milliseconds
const double ENCODER_PPR = 2400.0;

// Initialize variables
unsigned long previousMilliseconds = 0;
float previousCartPosition = 0;
float previousPendulumAngle = 0;

void setup()
{
  Serial.begin(9400);

  // Motor controller
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENA, OUTPUT);
}

void loop()
{
  unsigned long currentMilliseconds = millis();
  long cartEncoderCount = cartEncoder.read();
  long pendulumEncoderCount = pendulumEncoder.read();

  // Send values to python every n milliseconds
  if ( (currentMilliseconds - previousMilliseconds) > TIMEFRAME ) {

    // Compute the state
    stateVector state;
    state.pendulumAngle = encoderCountToAngleRadians(pendulumEncoderCount, ENCODER_PPR);              // radians
    state.cartPosition = encoderCountToCartPositionInches(cartEncoderCount, ENCODER_PPR);             // in
    state.pendulumAngularVelocity = (state.pendulumAngle - previousPendulumAngle)/(TIMEFRAME/1000.0); // radians/s
    state.cartVelocity = (state.cartPosition - previousCartPosition)/(TIMEFRAME/1000.0);              // in/s

    sendStateVectorToPython(state);

    // Store the current data for computation in the next loop
    previousMilliseconds = millis();
    previousPendulumAngle = state.pendulumAngle;
    previousCartPosition = state.cartPosition;

  }
}

pythonUtils.h

#ifndef PYTHON_UTILS
#define PYTHON_UTILS

#include <Arduino.h>

struct stateVector
{
    double pendulumAngle;
    double cartPosition;
    double pendulumAngularVelocity = 6.0;
    double cartVelocity = 5.0;
};


float encoderCountToAngleDegrees(long encoderCount);
float encoderCountToCartPositionInches(long cartEncoderCount, double encoderPPR);
void sendStateVectorToPython(stateVector state);


float encoderCountToAngleRadians(long encoderCount, double encoderPPR)
{
    return (encoderCount / encoderPPR) * (2.0 * PI);
}

float encoderCountToCartPositionInches(long cartEncoderCount, double encoderPPR) {
    float idlerPulleyRadius = 0.189;                                            // inches
    float cartAngle = encoderCountToAngleRadians(cartEncoderCount, encoderPPR); // radians
    return idlerPulleyRadius * cartAngle;
}

void sendStateVectorToPython(stateVector state)
{
    Serial.print(state.pendulumAngle);
    Serial.print(",");
    Serial.print(state.cartPosition);
    Serial.print(",");
    Serial.print(state.pendulumAngularVelocity);
    Serial.print(",");
    Serial.print(state.cartVelocity);
    Serial.println();
}

#endif
```
\$\endgroup\$

2 Answers 2

Reset to default
1
\$\begingroup\$

The Python program looks fairly good. I wouldn't call it bloated, but there are a few structural problems related to the Arduino class. They boil down to:

  • Interpreting the data received from the Arduino is done in the Arduino class, which will make it harder to reuse the code it the future, and makes the code harder to quickly read and understand.
  • The helper functions _has_digits and _check_for_overflowed_digits are only really useful in the Arduino class, so they should be static methods of the class.
  • In the program's current state, there is little reason for the Arduino class to exist. Instead, a single function would work just as well.

Interpreting the received data separately from reading it

In any program, you want each piece of code separated so that each piece only deals with the particular problem it is trying to solve. This helps to make it easier to reason about what a program is doing, as well as improve the ability for code to be reused in the future.

When looking at this program, the start_control is in violation of this idea. The method is performing two distinct actions:

  • Reading data from the serial port until a complete chunk of data is received.
  • Interpreting the data received from the first action.

Having these two tasks defined in the same function causes some problems. The first is that in order to figure out how the data is interpreted, you must also understand how the data is extracted from the serial port. In this program, you need to interpret the different branches of the if/else structure to figure out which path is executed when a complete chunk of data is available. The second problem is that it ties the function to exactly one way of interpreting the data, which is impossible to change without also changing the start_control function. In addition, it is impossible to reuse the code extracting data from the serial port without some nasty copy and pasting.

To fix this, place your interpreting code in a function:

def interpret_data(data):
   state_vector = _compute_state_vector(data)
   control_input = compute_input(state_vector)
   print(control_input)

and pass the function as a parameter to the Arduino class:

class Arduino():
    def __init__(self, action, port, baud_rate, timeout):
        self.interpret_func = action
        self.serial = serial.Serial(port, baud_rate, timeout = timeout)

    def start_control(self):

        last_line_recieved = ""

        try:
            while True:
                serial_bytes = self.serial.readline()
                decoded_line = str(serial_bytes.decode('utf8'))

                # Read from serial until we capture the whole line of data
                if (len(serial_bytes) != 0) and (b'\n' in serial_bytes):

                    # Sometimes digits overflow from one line onto the next
                    overflowed_digits = _check_for_overflowed_digits(decoded_line)
                    if overflowed_digits is not None:
                        last_line_recieved += overflowed_digits

                    # Use the data interpret function given in the constructor:
                    self.interpret_func(data)

                    # Reset the variable
                    last_line_recieved = ""

                elif len(serial_bytes) != 0:
                    last_line_recieved += decoded_line

                else:
                    pass

        except KeyboardInterrupt:
            print("You have quit reading from the serial port.")
            pass

if __name__ == "__main__":
    arduino = Arduino(interpret_data,
                      "/dev/cu.usbmodem14101", baud_rate = 9400, timeout = 0)
    arduino.start_control()

Use static methods

This one is fairly self-explanatory. Since _has_digits and _check_for_overflowed_digits are only used in the Arduino class, and do not modify the any of the Arduino class's variables, they should be static methods of the class:

class Arduino():
    @staticmethod
    def _has_digits(string):
        # implementation here
    @staticmethod
    def _check_for_overflowed_digits(string):
        # implementiation here

    # rest of Arduino class

Of course, you will need to prepend self in front of these methods when you call them.

Consider removing the Arduino class

This is more of a stylistic choice than anything else, but it in the way it is currently being used, the Arduino class doesn't provide any more utility than just having the function

def read_from_serial(action, port, baud_rate, timeout)

. Objects and classes are used to preserve data across function calls. Right now, the object is created, used once, and then never touched again. The data reuse properties that using an class provides is unused, making the Arduino class somewhat unneeded. If you were to connect to the same port multiple times after disconnecting from it, it may make sense to use an object to hold the data.

Other Observations

  • The serial port needs to be closed when you are done using serial.close(). In your case, it would probably go in the except KeyboardInterrupt: clause in the program.
  • start_control isn't a great name for the function. Consider using run_read_loop or something similar.
\$\endgroup\$
3
  • \$\begingroup\$ Awesome, thank you so much for the help!! I'm going to remove the Arduino class as you suggest. The goal of this program is to read data in from the Arduino every 100 ms and then once python receives the data, it's going to calculate a couple (expensive) things and send it back to the Arduino. This would be the interpret_data() in your example. Where should I write the data to the Serial port? Should this be done in interpret_data(), or should I create a separate function that takes an input argument and writes it to the serial port, something like def write_to_serial(string)? \$\endgroup\$ Commented Aug 25, 2019 at 14:58
  • \$\begingroup\$ More specifically, the writing to the serial port would replace the line print(control_input). \$\endgroup\$ Commented Aug 25, 2019 at 15:06
  • \$\begingroup\$ Ok so I have another question. Each person who passes in their action function is going to want to do different things with the data. Some will want to write back to the Arduino, others might just want to write the data to a CSV. That is, some want access to the serial port and others don't. How can the call interpret_func(data) be as reusable as possible? If only using functions, this would look like interpret_func(serial_port, data) which means every person must have a serial_port argument in their function. But I suppose it wouldn't be any different if using the class. \$\endgroup\$ Commented Aug 25, 2019 at 16:47
1
\$\begingroup\$

The 100ms time interval isn't.

The way loop() is coded, the loop isn't 100ms. It's 101ms plus the time to execute the code between the two calls to millis()

unsigned long currentMilliseconds = millis();
...
previousMilliseconds = millis();

The 101ms is because the test is:

if ( (currentMilliseconds - previousMilliseconds) > TIMEFRAME ) {

Maybe it doesn't matter, but to get a consistent interval it could be coded like so:

void loop()
{
  unsigned long currentMilliseconds = millis();
    ...
  if ( (currentMilliseconds - previousMilliseconds) >= TIMEFRAME ) {

    ...

    // this advances previousMilliseconds by the exact interval
    previousMilliseconds += TIMEFRAME;

    ...    
  }
}

A useful trick is to toggle an output bit in loop() or in the body of the if statement:

digitalWrite(LED_BUILTIN, ! digitalRead(LED_BUILTIN));

You can then monitor that pin with an o-scope or logic analyzer. Depending on where you put the toggle you can see how long it takes to run the loop, how often it runs, how much jitter there is in sending the data, etc.

\$\endgroup\$
1
  • \$\begingroup\$ This was really helpful, I didn't catch that at all. Thank you!! \$\endgroup\$ Commented Aug 28, 2019 at 15:19

You must log in to answer this question.

Start asking to get answers

Find the answer to your question by asking.

Ask question

Explore related questions

See similar questions with these tags.