Help fixing bug in code

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.

9 posts - 2 participants

Read full topic