pygame/arduino/pyserial - need help optimizing the arduino code.

Projects Programming
1

Ok, I am new to this forum, so I apologize for any errors in placement of this question.

I have been tasked with creating the code for an underwater robotics program, where we will be using 4 dc motors and 4 servos.

I have a pygame/python 3 script that takes input from one (will be two later on) Logitech extreme 3d pro joystick. I have converted the data from the four axis (X, Y, Z, and slider) to values between 1000-2000, to be used to control the motors and servos. Now I can successfully convert those data to ASCII and send it through to the arduino over a serial connection. However, the pygame window can only refresh once a second because of the non-optimized arduino code.

Now I am currently sending 46 bytes arranged like this: 1500,1500,1500,1500,0,0,0,0,0,0,0,0,0,0,0,0,0\n
Each one of the data that separated by a comma is the data that i have to parse and send to the individual motors and servos, with the exception of the 0's. Those are buttons and the 8-way hat switch (sending a value between 0-7).

Can anyone help me with the optimization of the arduino side of the code or if you see any mistakes in the python script of course let me know.

Thanks!

Due to the maximum amount of characters this website allows. I have attached the two files below.

main.txt (6.09 KB)

Serial_Tester.ino (1.9 KB)

2

OP's code:

#!python

import pygame
import serial
import time

# Define some colors
BLACK    = (   0,   0,   0)
WHITE    = ( 255, 255, 255)

# This is a simple class that will help us print to the screen
# It has nothing to do with the joysticks, just outputing the
# information.
class TextPrint:
    def __init__(self):
        self.reset()
        self.font = pygame.font.Font(None, 20)

    def plint(self, screen, textString):
        textBitmap = self.font.render(textString, True, BLACK)
        screen.blit(textBitmap, [self.x, self.y])
        self.y += self.line_height

    def reset(self):
        self.x = 10
        self.y = 10
        self.line_height = 15

    def indent(self):
        self.x += 10

    def unindent(self):
        self.x -= 10


pygame.init()

# Set the width and height of the screen [width,height]
size = [500, 700]
screen = pygame.display.set_mode(size)

pygame.display.set_caption("My Game")

#Loop until the user clicks the close button.
done = False

# Used to manage how fast the screen updates
clock = pygame.time.Clock()

# Initialize the joysticks
pygame.joystick.init()

# Get ready to print
textPrint = TextPrint()

startMarker = '<'
endMarker = '\n'
dividingmarker = ','

In_Min = float(-1)
In_Max = float(1)
Out_Min = float(1000)
Out_Max = float(2000)

arduino = serial.Serial(port="COM15", baudrate=115200, timeout=0.01)
time.sleep(.01)

# -------- Main Program Loop -----------
while done==False:
    # EVENT PROCESSING STEP
    for event in pygame.event.get(): # User did something
        if event.type == pygame.QUIT: # If user clicked close
            done=True # Flag that we are done so we exit this loop

    # DRAWING STEP
    # First, clear the screen to white. Don't put other drawing commands
    # above this, or they will be erased with this command.
    screen.fill(WHITE)
    textPrint.reset()

    # Get count of joysticks
    joystick_count = pygame.joystick.get_count()

    joystick1 = pygame.joystick.Joystick(0)
    joystick2 = pygame.joystick.Joystick(1)
    joystick1.init()
    joystick2.init()

    Joy1X = joystick1.get_axis(0)
    Joy1Y = joystick1.get_axis(1)
    Joy1Z = joystick1.get_axis(2)
    Joy1Slider = joystick1.get_axis(3)

    Joy2X = joystick2.get_axis(0)
    Joy2Y = joystick2.get_axis(1)
    Joy2Z = joystick2.get_axis(2)
    Joy2Slider = joystick2.get_axis(3)

    Joy1B1 = joystick1.get_button(0)
    Joy1B2 = joystick1.get_button(1)
    Joy1B3 = joystick1.get_button(2)
    Joy1B4 = joystick1.get_button(3)
    Joy1B5 = joystick1.get_button(4)
    Joy1B6 = joystick1.get_button(5)
    Joy1B7 = joystick1.get_button(6)
    Joy1B8 = joystick1.get_button(7)
    Joy1B9 = joystick1.get_button(8)
    Joy1B10 = joystick1.get_button(9)
    Joy1B11 = joystick1.get_button(10)
    Joy1B12 = joystick1.get_button(11)

    Joy2B1 = joystick2.get_button(0)
    Joy2B2 = joystick2.get_button(1)
    Joy2B3 = joystick2.get_button(2)
    Joy2B4 = joystick2.get_button(3)
    Joy2B5 = joystick2.get_button(4)
    Joy2B6 = joystick2.get_button(5)
    Joy2B7 = joystick2.get_button(6)
    Joy2B8 = joystick2.get_button(7)
    Joy2B9 = joystick2.get_button(8)
    Joy2B10 = joystick2.get_button(9)
    Joy2B11 = joystick2.get_button(10)
    Joy2B12 = joystick2.get_button(11)

    Joy1Hat = joystick1.get_hat(0)
    Joy2Hat = joystick2.get_hat(0)

    if Joy1Hat == (0,0):
        Joy1Hat_Send = 0
    elif Joy1Hat == (0,1):
        Joy1Hat_Send = 1
    elif Joy1Hat == (1,0):
        Joy1Hat_Send = 2
    elif Joy1Hat == (1,-1):
        Joy1Hat_Send = 3
    elif Joy1Hat == (0,-1):
        Joy1Hat_Send = 4
    elif Joy1Hat == (-1,-1):
        Joy1Hat_Send = 5
    elif Joy1Hat == (-1, 0):
        Joy1Hat_Send = 6
    elif Joy1Hat == (-1,1):
        Joy1Hat_Send = 7

    Joy1X_Send = (((Joy1X - In_Min) * (Out_Max - Out_Min)) / (In_Max - In_Min)) + 1000
    Joy1Y_Send = (((Joy1Y - In_Min) * (Out_Max - Out_Min)) / (In_Max - In_Min)) + 1000
    Joy1Z_Send = (((Joy1Z - In_Min) * (Out_Max - Out_Min)) / (In_Max - In_Min)) + 1000
    Joy1Slider_Send = (((Joy1Slider - In_Min) * (Out_Max - Out_Min)) / (In_Max - In_Min)) + 1000

    # JoyStick1_Data = (startMarker)
    JoyStick1_Data = str(round(Joy1X_Send))
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(round(Joy1Y_Send))
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(round(Joy1Z_Send))
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(round(Joy1Slider_Send))
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(Joy1B1)
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(Joy1B2)
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(Joy1B3)
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(Joy1B4)
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(Joy1B5)
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(Joy1B6)
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(Joy1B7)
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(Joy1B8)
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(Joy1B9)
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(Joy1B10)
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(Joy1B11)
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(Joy1B12)
    JoyStick1_Data += (dividingmarker)
    JoyStick1_Data += str(Joy1Hat_Send)
    JoyStick1_Data += (endMarker)

    JoyStick1_Data_ByteArray = bytearray(JoyStick1_Data, 'ascii')

#   1500,1500,1500,1500,0,0,0,0,0,0,0,0,0,0,0,0,0

    print(str(JoyStick1_Data_ByteArray))
    # arduino.write(JoyStick1_Data_ByteArray)
    # data = arduino.readline()
    # print(data + JoyStick1_Data_ByteArray)
    # ALL CODE TO DRAW SHOULD GO ABOVE THIS COMMENT

    # Go ahead and update the screen with what we've drawn.
    pygame.display.flip()

    # Limit to 20 frames per second
    clock.tick(1)

# Close the window and quit.
# If you forget this line, the program will 'hang'
# on exit if running from IDLE.
pygame.quit ()

Arduino code:

// This is very similar to Example 3 - Receive with start- and end-markers
//    in Serial Input Basics   http://forum.arduino.cc/index.php?topic=396450.0

const byte numChars = 64;
char receivedChars[numChars];

boolean newData = false;

byte ledPin = 13;   // the onboard LED

//===============

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

    pinMode(ledPin, OUTPUT);
    digitalWrite(ledPin, HIGH);
    delay(200);
    digitalWrite(ledPin, LOW);
    delay(200);
    digitalWrite(ledPin, HIGH);

    Serial.println("<Arduino is ready>");
}

//===============

void loop() {
    recvWithStartEndMarkers();
    replyToPython();
}

//===============

void recvWithStartEndMarkers() {
    static boolean recvInProgress = false;
    static byte ndx = 0;
    char startMarker = '<';
    char endMarker = '>';
    char rc;

    while (Serial.available() > 0 && newData == false) {
        rc = Serial.read();

        if (recvInProgress == true) {
            if (rc != endMarker) {
                receivedChars[ndx] = rc;
                ndx++;
                if (ndx >= numChars) {
                    ndx = numChars - 1;
                }
            }
            else {
                receivedChars[ndx] = '\0'; // terminate the string
                recvInProgress = false;
                ndx = 0;
                newData = true;
            }
        }

        else if (rc == startMarker) {
            recvInProgress = true;
        }
    }
}

//===============

void replyToPython() {
    if (newData == true) {
        Serial.print("<This just in ... ");
        Serial.print(receivedChars);
        Serial.print("   ");
        Serial.print(millis());
        Serial.print('>');
            // change the state of the LED everytime a reply is sent
        digitalWrite(ledPin, ! digitalRead(ledPin));
        newData = false;
    }
}

//===============

Please explain, in what way is the Arduino code "non-optimized". How do you know that it is responsible for the slowness? I don't see anything in there that is egregiously inefficient or slow.

3

At a baud rate of 115200, it takes about 4 msec to transfer your ~50 bytes which means the arduino can process ~250 messages per second given the code you provided.

I'm guessing the bulk of the time is spend actually doing something with that data, like writing to the motors, etc. but only you know that since you did not provide that code.

4

aarg: Whenever I increased the 'clock.tick()' line in the python script, the arduino can not keep up with parsing the data. If i increase that line to say 2 FPS, then it does not receive data (or output that data to the only servo i have attached).

blh64: The writing to the servos/motors uses the same writeMicroseconds as in the code i posted. I apologize for not saying this in there, I must've forgotten earlier today, but i do not have access to the motors and servos that we will be using so I only have one servo wired up to the arduino at the current moment. And since they are being written to the exact same way with values 1000-2000, i can switch which value does what. The last half of the data that i am sending to the arduino , " 0,0,0,0,0,0,0,0,0,0,0,0,0" are controlling relays and sorts. I am more worried about the arduino not receiving the data and parsing it in a sufficient amount of time. I am currently sending the data back to the python script for debugging. (if you have another way to do this, I am all ears)

Thanks to you both.

5

Did you post the correct ino file for the arduino? There is no code to control servos or motors, only echoing of the serial data received.

6

no i did not, i apologize. After working on this code for the last 48 hours, I have a lot of different files.

SerialTesting2.ino (3.16 KB)

7

A couple of things:

When you use SerialEvent(), do not call it from your code. The SerialEvent() function is called after the end of loop(), before beginning the next loop(), and is done automatically.

Serial.readBytesUntil() is a blocking function, it will wait until either the entire string is received, or it times out.

I'm not sure of your data format, you are using a semicolon as the terminating character in the latest code, in the first file you posted the deliminators were < and > . The receive method in your first file is also non-blocking, so you can do other things in your code while waiting for the serial data to arrive. Once you get a complete string from the input, then do the parsing in a separate function.

The parsing would also be a bit easier if you put the variables VA through VQ in an array.

You really shouldn't have a timing problem receiving the data more often, it takes about 17mS 3mS to do the parsing and drive the servo. <edit: had the timing wrong because of some extra print statements for debugging>

LacrosseKing's code:

#include <Servo.h>

Servo myServo;

int VA = 0; //0.0
int VB = 0; //0.0
int VC = 0; //1.0
int VD = 0; //0.0
int VE = 0; //0
int VF = 0; //0
int VG = 0; //0
int VH = 0; //0
int VI = 0; //0
int VJ = 0; //0
int VK = 0; //0
int VL = 0; //0
int VM = 0; //0
int VN = 0; //0
int VO = 0; //0
int VP = 0; //0
int VQ = 0; //0-8


void setup()
{
  Serial.begin(115200);
  myServo.attach(7);
}

void SerialEvent()
{
  char characterBuf[46];  //stores incoming
  int incomingLength = 0; //stores incoming length
  char *token;            //token for converting byte array to string array
  int counterNum = 1;


  if (Serial.available()) {
    incomingLength = Serial.readBytesUntil(';', characterBuf, 50);    //calculate length of byte array
//    Serial.println(incomingLength);
    token = strtok(characterBuf, ",");  //convert to string
    Serial.println(token);
    VA = atoi(token);
    //    Serial.println(token);
    while (token != NULL) {   //if token doesnt find another comma it goes back to begginning

      token = strtok(NULL, ",");  //changes token to a string def of NULL
      //      Serial.println(token);

      switch (counterNum) {
        case 1:
          VB = atoi(token);
          break;

        case 2:
          VC = atoi(token);
          break;

        case 3:
          VD = atoi(token);
          break;

        case 4:
          VE = atoi(token);
          break;

        case 5:
          VF = atoi(token);
          break;

        case 6:
          VG = atoi(token);
          break;

        case 7:
          VH = atoi(token);
          break;

        case 8:
          VI = atoi(token);
          break;

        case 9:
          VJ = atoi(token);
          break;

        case 10:
          VK = atoi(token);
          break;

        case 11:
          VL = atoi(token);
          break;

        case 12:
          VM = atoi(token);
          break;

        case 13:
          VN = atoi(token);
          break;

        case 14:
          VO = atoi(token);
          break;

        case 15:
          VP = atoi(token);
          break;

        case 16:
          VQ = atoi(token);
          break;
      }


      counterNum++;
    }
  }
}

void loop()
{
  if (Serial.available()) {
    SerialEvent();
    //call function
    Serial.print(VA);
    Serial.print("  ");
    Serial.print(VB);
    Serial.print("  ");
    Serial.print(VC);
    Serial.print("  ");
    Serial.print(VD);
    Serial.print("  ");
    Serial.print(VE);
    Serial.print("  ");
    Serial.print(VF);
    Serial.print("  ");
    Serial.print(VG);
    Serial.print("  ");
    Serial.print(VH);
    Serial.print("  ");
    Serial.print(VI);
    Serial.print("  ");
    Serial.print(VJ);
    Serial.print("  ");
    Serial.print(VK);
    Serial.print("  ");
    Serial.print(VL);
    Serial.print("  ");
    Serial.print(VM);
    Serial.print("  ");
    Serial.print(VN);
    Serial.print("  ");
    Serial.print(VO);
    Serial.print("  ");
    Serial.print(VP);
    Serial.print("  ");
    Serial.println(VQ);

  myServo.writeMicroseconds(VA);

  }
}
8

Instead of using a space-delimited series of values, it would be more efficient/robust to use a true serial packet system. You could implement one on your own or use the two compatible libraries pySerialTransfer and SerialTransfer.h.

pySerialTransfer is pip-installable and cross-platform compatible. SerialTransfer.h runs on the Arduino platform and can be installed through the Arduino IDE's Libraries Manager.

Example Python Script:

from time import sleep
from pySerialTransfer import pySerialTransfer as txfer

if __name__ == '__main__':
    try:
        link = txfer.SerialTransfer('COM13')
        
        link.open()
        sleep(2) # allow some time for the Arduino to completely reset
    
        link.txBuff[0] = 'h'
        link.txBuff[1] = 'i'
        link.txBuff[2] = '\n'
        
        link.send(3)
        
        while not link.available():
            if link.status < 0:
                print('ERROR: {}'.format(link.status))
            
        print('Response received:')
        
        response = ''
        for index in range(link.bytesRead):
            response += chr(link.rxBuff[index])
        
        print(response)
        link.close()
        
    except KeyboardInterrupt:
        link.close()

Example Arduino Sketch:

#include "SerialTransfer.h"

SerialTransfer myTransfer;

void setup()
{
  Serial.begin(115200);
  Serial1.begin(115200);
  myTransfer.begin(Serial1);
}

void loop()
{
  myTransfer.txBuff[0] = 'h';
  myTransfer.txBuff[1] = 'i';
  myTransfer.txBuff[2] = '\n';
  
  myTransfer.sendData(3);
  delay(100);

  if(myTransfer.available())
  {
    Serial.println("New Data");
    for(byte i = 0; i < myTransfer.bytesRead; i++)
      Serial.write(myTransfer.rxBuff[i]);
    Serial.println();
  }
  else if(myTransfer.status < 0)
  {
    Serial.print("ERROR: ");

    if(myTransfer.status == -1)
      Serial.println(F("CRC_ERROR"));
    else if(myTransfer.status == -2)
      Serial.println(F("PAYLOAD_ERROR"));
    else if(myTransfer.status == -3)
      Serial.println(F("STOP_BYTE_ERROR"));
  }
}

For theory behind robust serial communication, check out the tutorials Serial Input Basics and Serial Input Advanced.

9

David: I figured out that it was a synchronization error happening between the two codes. See the two attached files for completed code. Thank you for your help though.

Power_Broker: Thank you, I had no idea those two packages even existed. I'll look into them for sure when I implement more to the code. Thank you!!!

Thank you to you all for your help with this. It helped me out a lot!

SerialTesting2.ino (2.93 KB)

main.txt (3.89 KB)