Romi and RPi3 encoders reading error, same value as motors value

Hello, I am using the Romi 32U4 Control Board together with a Raspberry Pi 3 Model B, and the Romi Chassis and Romi Encoder Kit. I am using the RomiRPiSlaveDemo for the Romi 32U4 Control Board and I have defined the I2C slave and also I am using the function encoders.getCountsAndReset() and a delay time which will be the sample time of the Raspberry Pi, so the code changes as follows:

PololuRPiSlave<struct Data,20> slave;
.
.
.
void loop() {
  // Call updateBuffer() before using the buffer, to get the latest
  // data including recent master writes.
  slave.updateBuffer();
.
.
.
 slave.buffer.leftEncoder = encoders.getCountsAndResetLeft();
 slave.buffer.rightEncoder = encoders.getCountsAndResetRight();

  delayTime = slave.buffer.sampleTime;

  // When you are done WRITING, call finalizeWrites() to make modified
  // data available to I2C master.
  slave.finalizeWrites();

  delay(delayTime);
}

Then I have developed a python program based on the example/server.py but in my case I want to control the motors with the information from the encoders, so my code is as follows:

encoders = astar.read_encoders()
logging.debug("#CONTROL: [encL, encR] -> [{}, {}]".format(encoders[0], encoders[1]))
u = pid_calculate_new_value(encoders);
a_start.motors(int(u[0]), int(u[1]))
time.sleep(self.sample_period / 1000)

To control the motors I am using a PID controller and the output is the variable u that gets two values, for left and right motor, and the values are between -300 and 300.

My problem is that when I print the values of the encoders, sometimes is the real value and sometimes is the same value as the variable I am sending to the motors. I have try to put the delay between reading the encoders and writing to the motors, but I get the same result.

If I send a constant value of 100 to both motors, the result is the following:

2020-02-14 15:00:09,120 - DEBUG: #CONTROL: [encL, encR] -> [503, 507]
2020-02-14 15:00:09,687 - DEBUG: #CONTROL: [encL, encR] -> [100, 100]
2020-02-14 15:00:10,395 - DEBUG: #CONTROL: [encL, encR] -> [509, 514]
2020-02-14 15:00:11,030 - DEBUG: #CONTROL: [encL, encR] -> [100, 100]
2020-02-14 15:00:11,680 - DEBUG: #CONTROL: [encL, encR] -> [100, 100]
2020-02-14 15:00:12,272 - DEBUG: #CONTROL: [encL, encR] -> [513, 511]
2020-02-14 15:00:12,990 - DEBUG: #CONTROL: [encL, encR] -> [513, 512]
2020-02-14 15:00:13,628 - DEBUG: #CONTROL: [encL, encR] -> [515, 511]
2020-02-14 15:00:14,384 - DEBUG: #CONTROL: [encL, encR] -> [514, 514]
2020-02-14 15:00:14,981 - DEBUG: #CONTROL: [encL, encR] -> [100, 100]
2020-02-14 15:00:15,776 - DEBUG: #CONTROL: [encL, encR] -> [100, 100]
2020-02-14 15:00:16,489 - DEBUG: #CONTROL: [encL, encR] -> [510, 514]
2020-02-14 15:00:17,292 - DEBUG: #CONTROL: [encL, encR] -> [509, 515]
2020-02-14 15:00:18,019 - DEBUG: #CONTROL: [encL, encR] -> [508, 515]
2020-02-14 15:00:18,663 - DEBUG: #CONTROL: [encL, encR] -> [509, 516]
2020-02-14 15:00:19,237 - DEBUG: #CONTROL: [encL, encR] -> [100, 100]
2020-02-14 15:00:19,919 - DEBUG: #CONTROL: [encL, encR] -> [509, 518]
2020-02-14 15:00:20,565 - DEBUG: #CONTROL: [encL, encR] -> [511, 517]
2020-02-14 15:00:21,230 - DEBUG: #CONTROL: [encL, encR] -> [510, 518]
2020-02-14 15:00:21,927 - DEBUG: #CONTROL: [encL, encR] -> [100, 100]
2020-02-14 15:00:22,530 - DEBUG: #CONTROL: [encL, encR] -> [511, 519]
2020-02-14 15:00:23,245 - DEBUG: #CONTROL: [encL, encR] -> [511, 518]
2020-02-14 15:00:23,815 - DEBUG: #CONTROL: [encL, encR] -> [512, 520]
2020-02-14 15:00:24,574 - DEBUG: #CONTROL: [encL, encR] -> [513, 520]
2020-02-14 15:00:25,191 - DEBUG: #CONTROL: [encL, encR] -> [511, 521]
2020-02-14 15:00:25,840 - DEBUG: #CONTROL: [encL, encR] -> [100, 100]
2020-02-14 15:00:26,627 - DEBUG: #CONTROL: [encL, encR] -> [511, 520]
2020-02-14 15:00:27,422 - DEBUG: #CONTROL: [encL, encR] -> [513, 520]
2020-02-14 15:00:28,058 - DEBUG: #CONTROL: [encL, encR] -> [512, 518]
2020-02-14 15:00:28,630 - DEBUG: #CONTROL: [encL, encR] -> [513, 521]
2020-02-14 15:00:29,397 - DEBUG: #CONTROL: [encL, encR] -> [513, 519]
2020-02-14 15:00:30,169 - DEBUG: #CONTROL: [encL, encR] -> [100, 100]
2020-02-14 15:00:30,880 - DEBUG: #CONTROL: [encL, encR] -> [100, 100]

I think the problem is regarding the timing of reading and writing of the buffer, but I don’t know exactly how to fix it.

I hope I have given enough details and you can help me.

Thank you very much in advance!

Hello.

I suspect there is some mismatch between the structure of the data that the A-Star and the Raspberry Pi expect to see. Can you post your entire Arduino sketch along with the a_star.py for the Raspberry Pi?

- Amanda

Hello, thank you for the answer. My code for the Arduino is the following:

#include <Servo.h>
#include <Romi32U4.h>
#include <PololuRPiSlave.h>

struct Data
{
  bool yellow, green, red;
  bool buttonA, buttonB, buttonC;

  int16_t leftMotor, rightMotor;
  uint16_t batteryMillivolts;
  uint16_t analog[6];

  bool playNotes;
  char notes[14];

  int16_t leftEncoder, rightEncoder;

  uint16_t sampleTime;   // Defined in miliseconds
};

uint16_t delayTime = 1000;

PololuRPiSlave<struct Data,20> slave;
PololuBuzzer buzzer;
Romi32U4Motors motors;
Romi32U4ButtonA buttonA;
Romi32U4ButtonB buttonB;
Romi32U4ButtonC buttonC;
Romi32U4Encoders encoders;

void setup() {
  // Set up the slave at I2C address 20.
  slave.init(20);

  // Play startup sound.
  buzzer.play("v10>>g16>>>c16");
}

void loop() {
  // Call updateBuffer() before using the buffer, to get the latest
  // data including recent master writes.
  slave.updateBuffer();

  // Write various values into the data structure.
  slave.buffer.buttonA = buttonA.isPressed();
  slave.buffer.buttonB = buttonB.isPressed();
  slave.buffer.buttonC = buttonC.isPressed();

  // Change this to readBatteryMillivoltsLV() for the LV model.
  slave.buffer.batteryMillivolts = readBatteryMillivolts();

  for(uint8_t i=0; i<6; i++)
  {
    slave.buffer.analog[i] = analogRead(i);
  }

  // READING the buffer is allowed before or after finalizeWrites().
  ledYellow(slave.buffer.yellow);
  ledGreen(slave.buffer.green);
  ledRed(slave.buffer.red);
  motors.setSpeeds(slave.buffer.leftMotor, slave.buffer.rightMotor);

  // Playing music involves both reading and writing, since we only
  // want to do it once.
  static bool startedPlaying = false;
  
  if(slave.buffer.playNotes && !startedPlaying)
  {
    buzzer.play(slave.buffer.notes);
    startedPlaying = true;
  }
  else if (startedPlaying && !buzzer.isPlaying())
  {
    slave.buffer.playNotes = false;
    startedPlaying = false;
  }

  slave.buffer.leftEncoder = encoders.getCountsAndResetLeft();
  slave.buffer.rightEncoder = encoders.getCountsAndResetRight();

  delayTime = slave.buffer.sampleTime;

  // When you are done WRITING, call finalizeWrites() to make modified
  // data available to I2C master.
  slave.finalizeWrites();

  delay(delayTime);

}

My version of the a_star.py is the originial but adding the sample time:

# Copyright Pololu Corporation.  For more information, see https://www.pololu.com/
import smbus
import struct
import time


class AStar:
    def __init__(self):
        self.bus = smbus.SMBus(1)

    def read_unpack(self, address, size, format):
        # Ideally we could do this:
        #    byte_list = self.bus.read_i2c_block_data(20, address, size)
        # But the AVR's TWI module can't handle a quick write->read transition,
        # since the STOP interrupt will occasionally happen after the START
        # condition, and the TWI module is disabled until the interrupt can
        # be processed.
        #
        # A delay of 0.0001 (100 us) after each write is enough to account
        # for the worst-case situation in our example code.

        self.bus.write_byte(20, address)
        time.sleep(0.0001)
        byte_list = [self.bus.read_byte(20) for _ in range(size)]
        return struct.unpack(format, bytes(byte_list))

    def write_pack(self, address, format, *data):
        data_array = list(struct.pack(format, *data))
        self.bus.write_i2c_block_data(20, address, data_array)
        time.sleep(0.0001)

    def leds(self, red, yellow, green):
        self.write_pack(0, 'BBB', red, yellow, green)

    def play_notes(self, notes):
        self.write_pack(24, 'B15s', 1, notes.encode("ascii"))

    def motors(self, left, right):
        self.write_pack(6, 'hh', left, right)

    def read_buttons(self):
        return self.read_unpack(3, 3, "???")

    def read_battery_millivolts(self):
        return self.read_unpack(10, 2, "H")

    def read_analog(self):
        return self.read_unpack(12, 12, "HHHHHH")

    def read_encoders(self):
        return self.read_unpack(39, 4, 'hh')

    def test_read8(self):
        self.read_unpack(0, 8, 'cccccccc')

    def test_write8(self):
        self.bus.write_i2c_block_data(20, 0, [0, 0, 0, 0, 0, 0, 0, 0])
        time.sleep(0.0001)
    
    def define_sample_time(self, t_sample):
        self.write_pack(43, 'H', t_sample)

Thank you again.
David

Hi, David.

I tried to replicate your setup as closely as I could here and was unable to reproduce your issue. Can you run the benchmark.py script and post the results here? (This is to make sure there are no low-level I2C communication issues between the A-Star and the Raspberry Pi.)

What value did you use for sample_period in your Python script? What happens to the behavior when you increase and decrease that value? Can you simplify your Python program to a minimal example that demonstrates the issue and post it here?

- Amanda

Hi!

I have solved the problem. When I was writing a simple script to show you the problem, I realized everything was working fine, so I started making it more complex until I found the error.

In my original program, two threads are working at the same time, one reading the encoders and the other writing to the motors. The thread which is reading the encoders was running with a period of sample_period, the same as the Arduino program. But the other thread was writing much faster so they were reading and writing at the same time. As a solution, I am using locks when I am reading the encoders and writing to the motors, so the threads cannot use the I2C bus at the same time.

Also, my results after running benchmark.py is:

Writes of 8 bytes: 88.5 kilobits/second
Reads of 8 bytes: 49.5 kilobits/second

Thank you very much for your help.
David

2 Likes