Roboclaw Packet Serial Comm

Ah, apologies for dropping the ball on this one. I overcommitted and won't be able to complete this until most likely after the new ARC24 ARC/2 ARC.2 is released. Geez, we have to agree on a name soon! No one knows what to call it. But the new ARC will make implementing things like this much more manageable. The time frame for the new ARC is sometime in the new year. Albeit, porting the 700+ robot skills over will take a bit of time as well

I have a template project that I started on a while ago. I'll attach it here once I get in front of that laptop. I'm on a different PC right now.

@DJSures, Your last comment (Dec) mentioned that you were going to attach a template project file to the comment, but I don't see the attachment. Also, any update to when the Roboclaw port will be available?

Ah, crazy, I had the temp skill I was working on on my desktop in the fall - it looks like I deleted it at some point. If I come across it and I didn't delete it, I'll post it. But in the meantime, I'm swamped with ARCx development and don't have time for volunteer development. Your best bet is to tackle a robot skill. I'll try to answer questions, but I don't have much free time.

Oh, or you can reach out to the manufacturer of that product and ask them to contribute a robot skill - that's how they usually get created.

Looking at it real quick again to refresh my memory. I came across this 9-year-old driver that someone was working on for Python. It seems like the cleanest version of any examples I've seen using that product. I don't know why they made it so complicated and the driver so hard to use. I guess that's why it's such a rare device.

Here's the link: github.com/sonyccd/roboclaw_ros/blob/master/roboclaw_node/src/roboclaw_driver/roboclaw_driver.py

I asked gpt4 to convert that python and got this bit to test with...

using System;
using System.IO.Ports;
using System.Threading;

public class RoboController
{
    private SerialPort _port;
    private ushort _crc;
    private const int TrysTimeout = 3;

    // Enum for commands
    public enum Cmd
    {
        M1FORWARD = 0,
        M1BACKWARD = 1,
        SETMINMB = 2,
        SETMAXMB = 3,
        // Add all other commands here...
        GETPWMMODE = 149,
        FLAGBOOTLOADER = 255
    }

    // Constructor to open serial port
    public RoboController(string portName, int baudRate)
    {
        _port = new SerialPort(portName, baudRate);
        _port.Open();
    }

    // CRC calculation method
    private void CrcClear()
    {
        _crc = 0;
    }

    private void CrcUpdate(byte data)
    {
        _crc ^= (ushort)(data << 8);
        for (int i = 0; i < 8; i++)
        {
            if ((_crc & 0x8000) == 0x8000)
                _crc = (ushort)((_crc << 1) ^ 0x1021);
            else
                _crc <<= 1;
        }
    }

    // Example of sending a command
    private void SendCommand(byte address, Cmd command)
    {
        CrcClear();
        CrcUpdate(address);
        _port.Write(new byte[] { address }, 0, 1);
        CrcUpdate((byte)command);
        _port.Write(new byte[] { (byte)command }, 0, 1);
    }

    // Example command functions
    public bool ForwardM1(byte address, byte value)
    {
        try
        {
            SendCommand(address, Cmd.M1FORWARD);
            WriteByte(value);
            // Additional logic to handle command response and verification
            return true;
        }
        catch (Exception)
        {
            return false;
        }
    }

    private void WriteByte(byte val)
    {
        CrcUpdate(val);
        _port.Write(new byte[] { val }, 0, 1);
    }

    // Add other methods similar to the Python script for command handling and data reading

    // Close the port
    public void Close()
    {
        if (_port != null && _port.IsOpen)
            _port.Close();
    }

    // Main method for testing
    public static void Main(string[] args)
    {
        // Example usage
        RoboController controller = new RoboController("COM3", 38400);
        controller.ForwardM1(0x80, 0x64); // Example call
        controller.Close();
    }
}

And a few more tips - this will be super helpful. The template for the Sabertooth Kangaroo Movement Panel will provide a skeleton to drop in the commands from the robot claw.

Using that above conversion as a library to stub into the sabertooth serial class should provide movement quickly.

Here's the sabertooth mvoement panel source: Sabertooth Kangaroo Movement Panel.zip

Okay, I just spent the last hour after dinner staring at that Python script, and I take it back. It's probably the worst code I've ever seen in my life. There are 10-20 duplicate functions for every function. For some reason, the author copied functions repeatedly and gave them new names with the same code inside.

Everything about the roboclaw is poorly supported. I don't know what to tell you, but we understand why no one uses it. If you want to help that company by cleaning their code and making something work, my above example should do that. But it's going to be a HUGE job for you.

Here this should work as it's mostly complete...

using System.IO.Ports;

public class RoboClaw : IDisposable {

  private SerialPort _serialPort;
  private const byte Address = 0x80; // Default address, adjust as needed

  public RoboClaw(string portName, int baudRate = 38400) {

    _serialPort = new SerialPort(portName, baudRate, Parity.None, 8, StopBits.One);
    _serialPort.Open();
  }

  public void Dispose() {

    if (_serialPort.IsOpen)
      _serialPort.Close();
  }

  private ushort UpdateCrc(ushort crc, byte data) {

    crc ^= (ushort)(data << 8);

    for (int i = 0; i < 8; i++) {

      if ((crc & 0x8000) != 0)
        crc = (ushort)((crc << 1) ^ 0x1021);
      else
        crc <<= 1;
    }

    return crc;
  }

  private ushort calculateCrc(byte[] data) {

    ushort crc = 0;

    foreach (byte datum in data) {
      crc = UpdateCrc(crc, datum);
    }

    return crc;
  }

  private void sendCommand(byte command, byte[] data = null) {

    int dataLength = data != null ? data.Length : 0;
    byte[] buffer = new byte[1 + dataLength + 2]; // Address + Data + CRC
    int index = 0;

    buffer[index++] = Address;

    buffer[index++] = command;

    if (data != null) {
      Array.Copy(data, 0, buffer, index, dataLength);
    }

    ushort crc = calculateCrc(buffer[..(1 + dataLength)]);
    buffer[^2] = (byte)(crc >> 8);
    buffer[^1] = (byte)(crc & 0xFF);

    _serialPort.Write(buffer, 0, buffer.Length);
  }

  private byte[] readResponse(int size) {

    byte[] buffer = new byte[size + 2]; // Data + CRC
    int bytesRead = _serialPort.Read(buffer, 0, buffer.Length);

    if (bytesRead != buffer.Length) {
      throw new InvalidOperationException("Failed to read all bytes from RoboClaw");
    }

    // Verify CRC
    ushort calculatedCrc = calculateCrc(buffer[..^2]);
    ushort receivedCrc = (ushort)((buffer[^2] << 8) | buffer[^1]);

    if (calculatedCrc != receivedCrc) {
      throw new InvalidOperationException("CRC mismatch");
    }

    Array.Resize(ref buffer, buffer.Length - 2); // Remove CRC from the response

    return buffer;
  }

  public void ForwardM1(byte speed) {

    sendCommand(0, new byte[] { speed });
  }

  public void Close() {

    if (_serialPort != null && _serialPort.IsOpen) {
      _serialPort.Close();
    }
  }

  public void ForwardM1(int speed) {

    sendCommand(0, new byte[] { (byte)speed });
  }

  public void ForwardM2(int speed) {

    sendCommand(4, new byte[] { (byte)speed });
  }

  public void SetMaxCurrent(byte motor, uint current) {

    byte command = (byte)(motor == 1 ? 133 : 134);
    byte[] data = BitConverter.GetBytes(current);

    sendCommand(command, data);
  }

  public uint GetMaxCurrent(byte motor) {

    byte command = (byte)(motor == 1 ? 135 : 136);

    sendCommand(command);

    var data = readResponse(4);

    return BitConverter.ToUInt32(data, 0);
  }

  public long GetEncoderCount(byte motor) {

    byte command = (byte)(motor == 1 ? 16 : 17);

    sendCommand(command);

    var data = readResponse(4);

    return BitConverter.ToInt32(data, 0);
  }

  public void SetBatteryVoltages(float minVoltage, float maxVoltage) {

    byte[] minVoltageData = BitConverter.GetBytes((ushort)(minVoltage * 10));
    byte[] maxVoltageData = BitConverter.GetBytes((ushort)(maxVoltage * 10));

    sendCommand(57, minVoltageData);

    sendCommand(58, maxVoltageData);
  }

  public (float, float) GetBatteryVoltages() {

    sendCommand(24); // Get main battery voltage command
    var mainVoltageData = readResponse(2);
    float mainVoltage = BitConverter.ToUInt16(mainVoltageData, 0) / 10f;

    sendCommand(25); // Get logic battery voltage command
    var logicVoltageData = readResponse(2);
    float logicVoltage = BitConverter.ToUInt16(logicVoltageData, 0) / 10f;

    return (mainVoltage, logicVoltage);
  }

  public void SetSpeedAccel(byte motor, uint accel, int speed) {

    byte command = (byte)(motor == 1 ? 38 : 39);
    byte[] data = new byte[8];

    Array.Copy(BitConverter.GetBytes(accel), 0, data, 0, 4);
    Array.Copy(BitConverter.GetBytes(speed), 0, data, 4, 4);

    sendCommand(command, data);
  }
}