Help fixing bug in code

Projects Programming
1

Hi,

I've got some code setup where an Arduino receives a number of counts required from a Pi over serial. Some motors are being controlled by a Pi and the Arduino is being used to measure encoder pulses and send an output based on the number of pulses it has counted. This in turn will tell the Pi what speed to set the motors to and when to stop the motors.

I have found a bug, where the Arduino will receive the correct number of pulses required and then for some reason this value is set to zero. This causes the Pi to spin the motors endlessly because the Arduino just becomes stuck and so never sends the command to stop the motors.

I have looked into debugging but I'm not unfamiliar with Arduino debugging and eventually gave up when I found out it is not possible to use Serial communication with Platformio. I have no idea where to go with this now, so any advice would be much appreciated!

I'm using an Arduino mega2560 and using a single DC motor to generate pulses.

Here's the code running on the Pi:

import sys
import datetime
import time
import numpy as np 
import pandas as pd 
import serial
import motoron


# Global motor parameters
mc1 = motoron.MotoronI2C(address=17) # Bottom Board
mc2 = motoron.MotoronI2C(address=19) # Top Board

########################### Define Functions ###########################

def setup_motoron(mc):
  mc.reinitialize()
  mc.disable_crc()
  mc.clear_reset_flag()
  mc.disable_command_timeout()

def serial_comm(count, ser):
    time.sleep(5)
    ser.reset_input_buffer()
    ser.write(bytes(str(count) + '\n', 'utf-8'))
    received_data = ser.readline().decode('utf-8').strip()
    if received_data != str(count):
        print(f"Received data {received_data} does not match the expected count {count}. Retrying...")
        serial_comm(count,ser)  # Recursive call to send and verify again
    else:
        ser.reset_input_buffer()
        ser.write(bytes('OK\n', 'utf-8'))
        print("Data sent OK")
        return

def motor_control(robot_dist_dir):
  setup_motoron(mc1) 
  setup_motoron(mc2)
  speed_m = 500
  wheel_d = 0.120 #m
  enc_mult = 64 # maybe 32 if only on one edge rather than changing?
  gear_mult = 60 # 30:1 & 2:1
  req_counts = round((np.abs(robot_dist_dir)/(np.pi*wheel_d))*gear_mult*enc_mult)
  print(f'Counts: {req_counts}')
  # Motor 1, 2 & 3
  mc1.set_max_acceleration(1, 20) # (motorID,max accel)
  mc1.set_max_deceleration(1, 800) # (motorID,max decel)
  mc1.set_max_acceleration(2, 20) # (motorID,max accel)
  mc1.set_max_deceleration(2, 800) # (motorID,max decel)
  mc2.set_max_acceleration(1, 20) # (motorID,max accel)
  mc2.set_max_deceleration(1, 800) # (motorID,max decel)

  # Linear actuator
  mc2.set_max_acceleration(2, 10) # (motorID,max accel)
  mc2.set_max_deceleration(2, 10) # (motorID,max decel)

  ser = serial.Serial('/dev/ttyACM0', 115200, timeout=1)    
  serial_comm(req_counts,ser)
  
  # Change direction of speed_m if robot should travel backwards

  if robot_dist_dir < 0: 
    mc1.set_speed(1,-100)
    mc1.set_speed(2,-100)
    mc2.set_speed(1,-100)
  else:
    mc1.set_speed(1,100)
    mc1.set_speed(2,100)
    mc2.set_speed(1,100)     

  try:
    while True:
        progress = ser.readline().decode('utf-8').strip()
        if progress == "10":
            print('10%')
        elif progress == "20":
            print('20%')
        elif progress == "30":
            print('30%')
        elif progress == "40":
            print('40%')
        elif progress == "50":
            print('50%')
            if robot_dist_dir < 0: 
                mc1.set_speed(1,-50)
                mc1.set_speed(2,-50)
                mc2.set_speed(1,-50)
            else:
                mc1.set_speed(1,50)
                mc1.set_speed(2,50)
                mc2.set_speed(1,50) 
        elif progress == "60":
            print('60%')
        elif progress == "70":
            print('70%')
        elif progress == "80":
            print('80%')
            if robot_dist_dir < 0: 
                mc1.set_speed(1,-25)
                mc1.set_speed(2,-25)
                mc2.set_speed(1,-25)
            else:
                mc1.set_speed(1,25)
                mc1.set_speed(2,25)
                mc2.set_speed(1,25) 
        elif progress == "90":
            print('90%')
        elif progress == "100":
            mc1.set_speed(1,0)
            mc1.set_speed(2,0)
            mc2.set_speed(1,0)
            print('Complete')
            ser.close()
            break
  except KeyboardInterrupt:
    mc1.reinitialize()
    mc2.reinitialize()
    raise

  return 



robot_prev_dist = 6.0
motor_control(robot_prev_dist)
print('First movement complete')
time.sleep(10)

print(f'Moving: {robot_prev_dist*0.5}')
motor_control(robot_prev_dist*0.5)
print('Second movement complete')
time.sleep(10)

print(f'Moving: {robot_prev_dist*0.5}')
motor_control(robot_prev_dist*0.5)
print('Third movement complete')
time.sleep(10)

print(f'Moving: {robot_prev_dist*0.5}')
motor_control(robot_prev_dist*0.5)
print('Fourth movement complete')
time.sleep(10)

print(f'Moving: {robot_prev_dist*0.5}')
motor_control(robot_prev_dist*0.5)
print('Fifth movement complete')
time.sleep(10)

print(f'Moving: {robot_prev_dist*0.5}')
motor_control(robot_prev_dist*0.5)
print('Sixth movement complete')

Here's the code I'm running on the Arduino:

#include <Arduino.h>
#include <math.h>
#include <SoftwareSerial.h>

#define ENC_COUNT_REV 64
#define GEAR 60 //30:1 + 2:1 gear ratio
#define ENC_PIN_A 19
#define ENC_PIN_B 20
#define ARD_PI_PIN 18
#define rxPin 10
#define txPin 11

// MOTOR WIRING DATA
// Red - pos
// Black - Neg
// Green - GND
// Blue - Vcc (3.3v-20v)
// Yellow - Output A
// White - Output B


// Encoder counters
volatile long enc_a_count = 0;
volatile long enc_b_count = 0;
volatile long encoderCount = 0;
volatile long count; 

unsigned long reqCounts = 0;
unsigned long copy_count = 0;
unsigned long msec0; 
String data;

SoftwareSerial mySerial(rxPin, txPin);

void setup() {
  Serial.begin(9600);
  Serial.setTimeout(1);

    // Define pin modes for TX and RX
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);
    
    // Set the baud rate for the SoftwareSerial object
  mySerial.begin(115200);
  mySerial.setTimeout(1);

  pinMode(ENC_PIN_A, INPUT_PULLUP);
  pinMode(ENC_PIN_B, INPUT_PULLUP);


  attachInterrupt(digitalPinToInterrupt(ENC_PIN_A), updateEncoderA, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENC_PIN_B), updateEncoderB, CHANGE);

}


void loop() {

  reqCounts = 0;
  copy_count = 0;
  count = 0;
  data = "";

  while (true) {
    while (!mySerial.available());
    data = mySerial.readStringUntil('\n');
    delay(1000);
    if (data == "OK") {
      Serial.print("Data OK"); //debug
      Serial.println(); //debug
      mySerial.print("OK");
      mySerial.println();
      break;  // Break out of the loop if "OK" is received
    }
    else {
      reqCounts = strtoul(data.c_str(), NULL, 10);
      Serial.print("Received Counts: "); //debug
      Serial.print(reqCounts); //debug
      Serial.print('\t'); //debug
      Serial.println(); //debug
      mySerial.print(reqCounts);
      mySerial.println();
      //Serial.println();
    }
  }

 

  while (true) {
    unsigned long msec = millis();
    if (msec - msec0 >= 1000) {
    msec0 = msec;
    Serial.print(" Running Total pulses:");
    Serial.print(copy_count);
    Serial.print('\t');
    Serial.print(" Target pulses:");
    Serial.print(reqCounts);
    Serial.println();
    }
    

    noInterrupts();
    copy_count = count;
    interrupts();



    if (copy_count >=0.1*reqCounts && copy_count < 0.2*reqCounts ) {
    Serial.print("10");
    Serial.println();
    mySerial.print("10");
    mySerial.println();
    }
    if (copy_count >=0.2*reqCounts && copy_count < 0.3*reqCounts ) {
    Serial.print("20");
    Serial.println();
    mySerial.print("20");
    mySerial.println();
    }
    if (copy_count >= 0.3*reqCounts && copy_count < 0.4*reqCounts ) {
    Serial.print("30");
    Serial.println();
    mySerial.print("30");
    mySerial.println();
    }
    if (copy_count >= 0.4*reqCounts && copy_count < 0.5*reqCounts ) {
    Serial.print("40");
    Serial.println();
    mySerial.print("40");
    mySerial.println();
    }    
    if (copy_count >=0.5*reqCounts && copy_count < 0.6*reqCounts ) {
    Serial.print("50");
    Serial.println();
    mySerial.print("50");
    mySerial.println();
    }
    if (copy_count >=0.6*reqCounts && copy_count < 0.7*reqCounts ) {
    Serial.print("60");
    Serial.println();
    mySerial.print("60");
    mySerial.println();
    }
    if (copy_count >=0.7*reqCounts && copy_count < 0.8*reqCounts ) {
    Serial.print("70");
    Serial.println();
    }
    if (copy_count >=0.8*reqCounts && copy_count < 0.9*reqCounts ) {
    Serial.print("80");
    Serial.println();
    mySerial.print("80");
    mySerial.println();
    }
    if (copy_count >=0.9*reqCounts && copy_count < reqCounts ) {
    Serial.print("90");
    Serial.println();
    mySerial.print("90");
    mySerial.println();
    }
    if (copy_count > reqCounts) {
        // Set PIN to Low (STOP MOTORS)
      Serial.print("Complete");
      Serial.println();
      mySerial.print("Complete");
      mySerial.println();
      // Break out of the loop if "OK" is received
      break;
    }
  }

}


void updateEncoderA() {
  count++;
  enc_a_count++;
}

void updateEncoderB() {
  enc_b_count++;
  count++;
}

Here's the output from the serial monitor when the bug occurs:

17:44:07.330 -> Received Counts: 1273
17:44:13.334 -> Received Counts: 0

Edit: I have been playing around with the code in a different file and remembered the if (strtoul(data.c_str(), NULL, 10) > 0) {...} was just an else when I generated the output above. Apologies for any confusion. The output when the if statement is used is just:

15:48:13.894 -> Received Counts: 10186

Then the motors spin, but the Arduino is stuck.

2

From reference:
Serial.setTimeout() sets the maximum milliseconds to wait for serial data. It defaults to 1000 milliseconds.

You're running at 9600, which equates to 1 char/ms, close enough.
Every now and then, you call for characters, but get none within 1 ms, so the return is zero.
Not sure if this is your only problem, no time right now, but I'd set that to 5 ms and see if the problem goes away.
Also, you're not checking for a zero value received that I can see. Build in a trap for that in your code!

3

Still getting the same result after setting the Serial.setTimeout() to 5ms. I've built a trap in the Arduino code and I'm going to try and get the Pi to resend the data when the Arduino falls into it.

4

Okay, but now we have some confidence it's not a timeout issue.
Now, please explain

delay(1000);

Why do you need it?

5

Sorry that was just from me trying things. It didn't work.

6

You'll have to post a new version now, I have no confidence we're looking at the same code any more.

7

Sorry I don't understand, all I have done is simplify the code (primarily the Pi code) so that it doesn't take so long between cycles. I.e., I want to generate the issue faster so that I can try and fix it.

8

Okay, so is the delay() in your current version or not? This is what I mean. You post a code, want us to debug it, and then when I ask about the delay, you say it's not there, or "it didn't work"?

9

I'll create a separate sketch for my testing. Sorry

10

Sorry, but apparently either I'm not speaking English, or you don't want to. The issue is, you posted a code. I questioned the presence of a delay statement I see in the code. You seem to be indicating it's not in the code, or didn't work, but that's not what you posted. So I'll ask one more time. Is what you posted the exact code that exhibits the problem you're asking for help with? Because if it is, the first thing we need to determine is why you feel a delay is necessary at that point in the code. What does it do for you? When you say didn't work, are you saying you still see zero sometimes, or the entire code failed utterly, or something else?

11

The code I posted exhibits the problem described in the first post. A separate issue I was having with serial communication was solved by adding a delay. So, I added a delay to see if there was some timing issue. When I run the code, with or without the delay, I still get the exactly the same issue described in the first post. The delay is in the code.

12

Where? I do not see the code.

13
  1. software Serial at 115200? No way. max 38400, better 19200.
  2. that long series of if statements can be replaced by something similar to:
    int val = map(copy_count, 0, reqCounts, 0, 10));
    serial.println(val);
    mySerial.println(val);

If the timeouts are still at 1 ms, change them to 5. It may or may not be causing you problems, but it's unnecessary.
You can push Serial to 115200; again, may not be necessary, but your serial output is a huge drag.

14

The first while loop:

  while (true) {
    while (!mySerial.available());
    data = mySerial.readStringUntil('\n');
    delay(1000); ...}
15

And, don't set count=0 at the top of loop(), because that counter is being handled by the interrupt.

16

Timeouts are set to 5ms. What's the benefit of reducing baud rate? Will make changes based on point 2. :+1:

17

Reset counts in one of the conditional statements?

18

It's a 32bit uint. The clearing is not atomic, so your interrupt could happen part way through the clearing. You need to clear and set, like you did elsewhere for the copy:

    noInterrupts();
    count=0;
    interrupts();
19

I think what may be happening is that despite the motors being stationary, the encoder is still triggering interrupts. I say this because the serial output after receiving reqCounts from the Pi but prior to the motor spinning up there are some counts which have been recorded. Here's the output:

18:28:20.745 -> Recieved Counts: 1273	
18:28:21.738 -> Data OK
18:28:21.770 ->  Running Total pulses:0	 Target pulses:1273
18:28:22.743 ->  Running Total pulses:35	 Target pulses:1273
18:28:23.768 ->  Running Total pulses:35	 Target pulses:1273
18:28:24.746 ->  Running Total pulses:35	 Target pulses:1273
18:28:25.768 ->  Running Total pulses:35	 Target pulses:1273
18:28:26.742 ->  Running Total pulses:35	 Target pulses:1273
18:28:27.765 ->  Running Total pulses:35	 Target pulses:1273

The counts are reset prior to each serial message so should be 0, the first running total pulse is 0 because it comes before the:

    noInterrupts();
    copy_count = count;
    interrupts();

What do you think? Is there an easy fix? I think I read that interrupts must be on for serial communication so I couldn't just do a noInterrupts();, right?

20

By the way, why software serial when the Mega has 4 hardware serial ports?