It sounds like you're experiencing issues with the mapping accuracy when using The Better Navigator with your Roomba and encoder setup. Let's go through some steps to troubleshoot and potentially resolve the issue:

1. Verify Encoder Configuration:

   • Ensure that the encoder counts are correctly configured in the Wheel Encoder Counter skill. Double-check the wheel diameter and counts per revolution settings to match your Roomba's specifications.

2. Check Encoder Wiring:

   • Verify that the wiring from the Roomba encoders to the Arduino is secure and correct. Any loose connections can lead to inaccurate readings.

3. Review Wheel Encoder Counter Settings:

   • Make sure the "Push to NMS" option is enabled, as you mentioned. This ensures that the encoder data is sent to the Navigation Messaging System (NMS).
   • Confirm that the encoder counts are being updated correctly in real-time. You can use the Variable Watcher skill to monitor the encoder values.

4. Pose Hint Source:

   • Since you're using "External" as the pose hint source, ensure that the encoder data is reliable. If the encoder data is not accurate, it can lead to incorrect pose estimation.
   • Consider testing with "Differential" as the pose hint source to see if it improves the mapping accuracy. This option uses both the external sensor and the internal Hector SLAM calculation.

5. Lidar Configuration:

   • Ensure that the RPLidar is correctly configured and providing accurate distance data. The lidar data is crucial for mapping and obstacle detection.

6. Test with Different Movement Speeds:

   • Try reducing the movement speed of the Roomba when turning. High speeds can lead to inaccuracies in encoder readings and mapping.

7. Check for Software Updates:

   • Ensure that you are using the latest version of ARC and all robot skills. Updates may include bug fixes and improvements.

8. Debugging:

   • Enable the "Navigation debugging" option in The Better Navigator's configuration to get more detailed logs about the navigation process. This can help identify where the issue might be occurring.

9. Consult the Manuals:

   • Review the manuals for The Better Navigator and the Wheel Encoder Counter skill to ensure all settings are correctly configured.

If you've gone through these steps and the issue persists, consider reaching out to Synthiam support for further assistance. They may be able to provide more specific guidance based on your setup. Additionally, you can explore the possibility of using a different pose hint source or adjusting the configuration settings to better suit your environment and hardware.

Thank You, @Athena, Tried all Pose Hint Sources but the result seems to be the same. In the first screen capture, I just go forward. In the second, I turn left, and the map goes berserk.

It seems like the issue might be related to how the encoder data is being interpreted during turns. Here are a few additional steps you can take to troubleshoot and potentially resolve the issue:

1. Calibration of Encoders:

   • Double-check the calibration of your encoders. Ensure that the counts per revolution and wheel diameter are accurately set. Any discrepancies here can lead to incorrect distance and turn calculations.

2. Encoder Data Validation:

   • Use the Variable Watcher skill to monitor the encoder values in real-time. Check if the values make sense when you perform forward and turning movements. Look for any sudden spikes or drops in the encoder counts.

3. Review Arduino Firmware:

   • Ensure that the Arduino firmware for the Wheel Encoder UNO is correctly processing and sending the encoder data. Verify that the data being sent to ARC is accurate and consistent.

4. Check for Slippage:

   • Ensure that the Roomba's wheels are not slipping during turns. Slippage can cause the encoders to report incorrect values, leading to mapping errors.

5. Adjust Turning Speed:

   • Try reducing the turning speed of the Roomba. High speeds during turns can lead to inaccuracies in encoder readings and mapping.

6. Lidar Data Correlation:

   • Ensure that the RPLidar data is correctly aligned with the encoder data. The lidar should provide consistent obstacle detection and mapping data.

7. Debugging Logs:

   • Enable detailed debugging logs in The Better Navigator to see if there are any errors or warnings during the mapping process. This can provide insights into what might be going wrong.

8. Cross-Check with Other Skills:

   • If possible, test the encoder data with other skills or scripts to see if the issue is isolated to The Better Navigator or if it's a broader problem with the encoder data.

9. Consult Synthiam Support:

   • If the issue persists, consider reaching out to Synthiam support with detailed logs and descriptions of the problem. They may have additional insights or solutions based on your specific setup.

10. Alternative Mapping Skills:

    • As a last resort, you might want to try using a different mapping or navigation skill to see if it handles the encoder data differently.

By following these steps, you should be able to narrow down the cause of the issue and find a solution. If you have any more questions or need further assistance, feel free to ask!

@Athena,

One thing I noticed is that no matter if I move forward or backward the encoder counter skill counts always up to positive numbers. Shouldnt it be subtracting counts when moving backward?

Yes, you are correct. When moving backward, the encoder counts should typically decrease, resulting in negative values. If the encoder counter skill is only counting up, regardless of the direction, it could indicate a configuration issue. Here are some steps to address this:

1. Check Encoder Wiring:

   • Ensure that the wiring from the Roomba encoders to the Arduino is correct. If the wiring is reversed, it might cause the encoder to count in the same direction for both forward and backward movements.

2. Review Arduino Firmware:

   • Verify that the Arduino firmware is correctly interpreting the encoder signals. The firmware should differentiate between forward and backward movements and adjust the counts accordingly.

3. Encoder Counter Skill Configuration:

   • Double-check the settings in the Wheel Encoder Counter skill. Ensure that the configuration matches the expected behavior of your encoders.
   • Look for any options related to direction or inversion that might need adjustment.

4. Test with Simple Movements:

   • Perform simple forward and backward movements while monitoring the encoder values. This can help you identify if the issue is consistent or intermittent.

5. Debugging:

   • Use the Variable Watcher skill to monitor the encoder values in real-time. This can help you see how the values change with different movements.

6. Consult the Encoder Manual:

   • If available, refer to the manual or documentation for your specific encoders. There might be specific instructions or settings related to direction handling.

7. Firmware Update:

   • Ensure that the Arduino firmware is up-to-date. There might be updates or patches that address this issue.

8. Contact Support:

   • If the issue persists, consider reaching out to Synthiam support or the community for further assistance. They might have encountered similar issues and can provide additional guidance.

By addressing these points, you should be able to resolve the issue with the encoder counts not reflecting the correct direction. If you have any more questions or need further assistance, feel free to ask!

@Athena, Please take a look at the code from DJ's UNO Firmware with Wheel Encoder Counter; it only counts up. How would you proceed to modify the code to take into account all 4 movement directions?

#include 
#include "SendOnlySoftwareSerial.h"

// The first digital port that is usable on this controller
#define _PortStart 4

// The last digital port on this controller that is usable
#define _PortCnt 14

// The number of analog ports
#define _AnalogPorts 6

// The firmware version that is reported to ARC to notify of capabilities
#define _FIRMWARE_ID 0x0000000B

// The communication baud rate
#define _BAUD_RATE 57600

// The primary communication interface between ARC and this controller
#define COMMUNICATION_PORT Serial

// The amount of RX buffer on the communication interface for EZ-Builder
#define _BUFFER_SIZE 1024

// --------------------------------------------------------------------------

byte         _INPUT_BUFFER[_BUFFER_SIZE];
unsigned int _WRITE_POSITION = 0;
unsigned int _READ_POSITION = 0;
unsigned int _WheelLeftCnt = 0;
unsigned int _WheelRightCnt = 0;

int _BAUD_RATES [] = {
  4800,
  9600,
  19200,
  38400,
  57600,
  115200,
  115200
};

Servo Servos[_PortCnt];

#define CmdOurCustomCmds      0
#define CmdReleaseAllServos   1
#define CmdGetUniqueID        2
#define CmdEZBv3              3
#define CmdEZBv4              4
#define CmdSoundBeep          5
#define CmdEZServo            6
#define CmdI2CWrite           10
#define CmdI2CRead            11
#define CmdBootLoader         14
#define CmdSetPWMSpeed        15
#define CmdSetServoSpeed      39
#define CmdPing               0x55
#define CmdSetDigitalPortOn   100
#define CmdSetDigitalPortOff  124
#define CmdGetDigitalPort     148
#define CmdSetServoPosition   172
#define CmdGetADCValue        196
#define CmdSendSerial         204
#define CmdHC_SR04            228
#define CmdGetFirwareID       253
#define CmdSoundStreamCmd     254

// CmdEZBv4 Commands
// ----------------------------------------------------------------------------------
#define CmdV4SetLipoBatteryProtectionState 0
#define CmdV4SetBatteryMonitorVoltage      1
#define CmdV4GetBatteryVoltage             2
#define CmdV4GetCPUTemp                    3

#define CmdV4UARTExpansion0Init            5
#define CmdV4UARTExpansion0Write           6
#define CmdV4UARTExpansion0AvailableBytes  7
#define CmdV4UARTExpansion0Read            8

#define CmdV4UARTExpansion1Init            9
#define CmdV4UARTExpansion1Write           10
#define CmdV4UARTExpansion1AvailableBytes  11
#define CmdV4UARTExpansion1Read            12

#define CmdV4UARTExpansion2Init            13
#define CmdV4UARTExpansion2Write           14
#define CmdV4UARTExpansion2AvailableBytes  15
#define CmdV4UARTExpansion2Read            16

#define CmdV4I2CClockSpeed                 17
#define CmdV4UARTClockSpeed                18
#define CmdV4ResetToDefaults               19

// CmdSoundStreamCmd Commands
// ----------------------------------------------------------------------------------
#define CmdSoundInitStop 0
#define CmdSoundLoad     1
#define CmdSoundPlay     2

bool IsAvail() {

  return _WRITE_POSITION != _READ_POSITION || COMMUNICATION_PORT.available();
}

byte ReadByte() {

  while (_WRITE_POSITION == _READ_POSITION && COMMUNICATION_PORT.available() == 0);

  while (COMMUNICATION_PORT.available()) {

    _WRITE_POSITION++;

    _INPUT_BUFFER[_WRITE_POSITION % _BUFFER_SIZE] = COMMUNICATION_PORT.read();
  }

  _READ_POSITION++;

  return _INPUT_BUFFER[_READ_POSITION % _BUFFER_SIZE];
}

void setup() {

  COMMUNICATION_PORT.begin(_BAUD_RATE);

  // Left Wheel
  pinMode(2, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(2), countLeft, CHANGE);

  // Right Wheel
  pinMode(3, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(3), countRight, CHANGE);
}

void countLeft()  {

  _WheelLeftCnt++;
}

void countRight() {

  _WheelRightCnt++;
}

void loop() {

  doEZProtocol();
}

void Write32(long val) {

  COMMUNICATION_PORT.write((byte)(val & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 8) & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 16) & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 24) & 0xff));
}

void Write16(int val) {

  COMMUNICATION_PORT.write((byte)(val & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 8) & 0xff));
}

#define UNKNOWN_PIN 0xFF

uint8_t getPinMode(uint8_t pin) {

  uint8_t bit = digitalPinToBitMask(pin);
  uint8_t port = digitalPinToPort(pin);

  // I don't see an option for mega to return this, but whatever...
  if (NOT_A_PIN == port)
    return UNKNOWN_PIN;

  // Is there a bit we can check?
  if (0 == bit)
    return UNKNOWN_PIN;

  // Is there only a single bit set?
  if (bit & bit - 1)
    return UNKNOWN_PIN;

  volatile uint8_t *reg, *out;
  reg = portModeRegister(port);
  out = portOutputRegister(port);

  if (*reg & bit)
    return OUTPUT;
  else if (*out & bit)
    return INPUT_PULLUP;
  else
    return INPUT;
}

void doEZProtocol() {

  if (IsAvail()) {

    byte cmd = ReadByte();

    if (cmd == CmdPing) {

      // return as a "Capability Controller"
      COMMUNICATION_PORT.write(222);
    } else if (cmd == CmdGetFirwareID) {

      Write32(_FIRMWARE_ID);
    } else if (cmd == CmdReleaseAllServos) {

      for (int port = _PortStart; port < _PortCnt; port++)
        if (Servos[port].attached())
          Servos[port].detach();
    } else if (cmd >= CmdSetServoPosition && cmd <= CmdSetServoPosition + 23) {

      byte port = cmd - CmdSetServoPosition;
      byte pos = ReadByte();

      if (port >= _PortStart && port <= _PortCnt) {

        if (pos == 0 && Servos[port].attached()) {

          Servos[port].detach();
        } else {

          if (!Servos[port].attached())
            Servos[port].attach(port);

          Servos[port].write(pos);
        }
      }
    } else if (cmd >= CmdSetPWMSpeed && cmd <= CmdSetPWMSpeed + 23) {

      byte port = cmd - CmdSetPWMSpeed;
      byte pos = ReadByte();

      if (port >= _PortStart && port <= _PortCnt) {

        if (Servos[port].attached())
          Servos[port].detach();

        if (getPinMode(port) != OUTPUT)
          pinMode(port, OUTPUT);

        analogWrite(port, map(pos, 0, 100, 0, 255));
      }
    } else if (cmd >= CmdSetDigitalPortOn && cmd <= CmdSetDigitalPortOn + 23) {

      byte port = cmd - CmdSetDigitalPortOn;

      if (port >= _PortStart && port <= _PortCnt) {

        if (Servos[port].attached())
          Servos[port].detach();

        if (getPinMode(port) != OUTPUT)
          pinMode(port, OUTPUT);

        digitalWrite(port, HIGH);
      }
    } else if (cmd >= CmdSetDigitalPortOff && cmd <= CmdSetDigitalPortOff + 23) {

      byte port = cmd - CmdSetDigitalPortOff;

      if (port >= _PortStart && port <= _PortCnt) {

        if (Servos[port].attached())
          Servos[port].detach();

        if (getPinMode(port) != OUTPUT)
          pinMode(port, OUTPUT);

        digitalWrite(port, LOW);
      }
    } else if (cmd >= CmdGetDigitalPort && cmd <= CmdGetDigitalPort + 23) {

      byte port = cmd - CmdGetDigitalPort;

      if (port >= _PortStart && port <= _PortCnt) {

        if (Servos[port].attached())
          Servos[port].detach();

        if (getPinMode(port) != INPUT)
          pinMode(port, INPUT);

        COMMUNICATION_PORT.write(digitalRead(port));
      } else {

        COMMUNICATION_PORT.write(0);
      }
    } else if (cmd >= CmdGetADCValue && cmd <= CmdGetADCValue + 7) {

      byte port = cmd - CmdGetADCValue;

      if (port >= 0 && port <= _AnalogPorts)
        Write16(analogRead(port));
      else
        Write16(0);
    } else if (cmd >= CmdSendSerial && cmd <= CmdSendSerial + 23) {

      // Send Serial
      uint8_t port = cmd - CmdSendSerial;
      uint8_t baud = ReadByte(); // Baud rate
      uint8_t size = ReadByte(); // Size

      if (port >= _PortStart && port <= _PortCnt) {

        if (Servos[port].attached())
          Servos[port].detach();

        SendOnlySoftwareSerial tmpSerial(port);

        tmpSerial.begin(_BAUD_RATES[baud]);

        for (int x = 0; x < size; x++)
          tmpSerial.write(ReadByte());

        tmpSerial.end();
      }
    } else if (cmd == CmdOurCustomCmds) {

      byte cmd2 = ReadByte();

      if (cmd2 == 0x00) {

        // Get Both Wheel Counts
        Write16(_WheelLeftCnt);
        Write16(_WheelRightCnt);
      } else if (cmd2 == 0x01) {

        // Get Both Wheel Counts & Reset
        Write16(_WheelLeftCnt);
        Write16(_WheelRightCnt);

        _WheelLeftCnt = 0;
        _WheelRightCnt = 0;
      } else if (cmd2 == 0x02) {
      
        // Reset Wheel Counts
        _WheelLeftCnt = 0;
        _WheelRightCnt = 0;
      }
    }
  }
}

Quote:

Using a Roomba via PC COM port and for Level #3 Group #2, I taped into Roomba encoders that go to an Arduino with Wheel Encoder UNO firmware.

Quote:

For Level #3 Group #1, I use a RPLidar. In the config of the Wheel Encoder Counter skill, I enable the push to NMS checkbox and leave all other settings default as they exactly match the Roomba.

To modify the code to account for all four movement directions, you'll need to implement a way to determine the direction of movement. Since the encoder only provides counts and not direction, you can use additional logic to infer direction based on the commands sent to the robot. Here's a general approach to modify the code:

1. Track Movement Direction:

   • You need to track the current movement direction (forward, backward, left, right) in your code. This can be done by setting a variable whenever a movement command is issued.

2. Adjust Encoder Counts Based on Direction:

   • Modify the countLeft() and countRight() functions to adjust the counts based on the current direction. For example, if the robot is moving backward, decrement the count instead of incrementing it.

Here's a modified version of the code with these changes:

#include <Servo.h>
#include <SendOnlySoftwareSerial.h>

// The first digital port that is usable on this controller
#define _PortStart 4
// The last digital port on this controller that is usable
#define _PortCnt 14
// The number of analog ports
#define _AnalogPorts 6
// The firmware version that is reported to ARC to notify of capabilities
#define _FIRMWARE_ID 0x0000000B
// The communication baud rate
#define _BAUD_RATE 57600
// The primary communication interface between ARC and this controller
#define COMMUNICATION_PORT Serial
// The amount of RX buffer on the communication interface for EZ-Builder
#define _BUFFER_SIZE 1024

// Movement directions
enum Direction { FORWARD, BACKWARD, LEFT, RIGHT, STOP };
Direction currentDirection = STOP;

byte _INPUT_BUFFER[_BUFFER_SIZE];
unsigned int _WRITE_POSITION = 0;
unsigned int _READ_POSITION = 0;
int _WheelLeftCnt = 0;
int _WheelRightCnt = 0;

void setup() {
    COMMUNICATION_PORT.begin(_BAUD_RATE);

    // Left Wheel
    pinMode(2, INPUT_PULLUP);
    attachInterrupt(digitalPinToInterrupt(2), countLeft, CHANGE);

    // Right Wheel
    pinMode(3, INPUT_PULLUP);
    attachInterrupt(digitalPinToInterrupt(3), countRight, CHANGE);
}

void countLeft() {
    if (currentDirection == FORWARD || currentDirection == RIGHT) {
        _WheelLeftCnt++;
    } else if (currentDirection == BACKWARD || currentDirection == LEFT) {
        _WheelLeftCnt--;
    }
}

void countRight() {
    if (currentDirection == FORWARD || currentDirection == LEFT) {
        _WheelRightCnt++;
    } else if (currentDirection == BACKWARD || currentDirection == RIGHT) {
        _WheelRightCnt--;
    }
}

void loop() {
    doEZProtocol();
}

void doEZProtocol() {
    if (IsAvail()) {
        byte cmd = ReadByte();
        if (cmd == CmdPing) {
            COMMUNICATION_PORT.write(222);
        } else if (cmd == CmdGetFirwareID) {
            Write32(_FIRMWARE_ID);
        } else if (cmd == CmdOurCustomCmds) {
            byte cmd2 = ReadByte();
            if (cmd2 == 0x00) {
                Write16(_WheelLeftCnt);
                Write16(_WheelRightCnt);
            } else if (cmd2 == 0x01) {
                Write16(_WheelLeftCnt);
                Write16(_WheelRightCnt);
                _WheelLeftCnt = 0;
                _WheelRightCnt = 0;
            } else if (cmd2 == 0x02) {
                _WheelLeftCnt = 0;
                _WheelRightCnt = 0;
            }
        }
    }
}

bool IsAvail() {
    return _WRITE_POSITION != _READ_POSITION || COMMUNICATION_PORT.available();
}

byte ReadByte() {
    while (_WRITE_POSITION == _READ_POSITION && COMMUNICATION_PORT.available() == 0);
    while (COMMUNICATION_PORT.available()) {
        _WRITE_POSITION++;
        _INPUT_BUFFER[_WRITE_POSITION % _BUFFER_SIZE] = COMMUNICATION_PORT.read();
    }
    _READ_POSITION++;
    return _INPUT_BUFFER[_READ_POSITION % _BUFFER_SIZE];
}

void Write32(long val) {
    COMMUNICATION_PORT.write((byte)(val & 0xff));
    COMMUNICATION_PORT.write((byte)((val >> 8) & 0xff));
    COMMUNICATION_PORT.write((byte)((val >> 16) & 0xff));
    COMMUNICATION_PORT.write((byte)((val >> 24) & 0xff));
}

void Write16(int val) {
    COMMUNICATION_PORT.write((byte)(val & 0xff));
    COMMUNICATION_PORT.write((byte)((val >> 8) & 0xff));
}

Additional Steps:

• Direction Tracking: You need to implement a way to update the currentDirection variable based on the movement commands sent to the robot. This might involve modifying the communication protocol to include direction information.
• Testing: After implementing these changes, test the robot's movement and encoder counts to ensure they reflect the correct direction.

If you have any more questions or need further assistance, feel free to ask!