Using a Microcontroller as an Intermediary - SyncWrite and BulkRead

First, run this skecth on your openCM:

4KB

20220208_Passthrough.ino

Then, run this script in Matlab to syncWrite and bulkRead from 3 servos as they oscillate:

if exist("port") %#ok<EXIST> 
    %Do not make a new serialport object, just flush this one.
    port.flush();
else
    %Make a new serialport object.
    comport = "COM4";
    baud = double(1000000);
    port = serialport(comport,baud);
end

toRead = 2;
toWrite = 3;
toSyncWrite = 131; %0x83

axGoalPositionAddr = 30;
mxPresentPositionAddr = 36;

ID1 = 0;
ID2 = 1;
ID3 = 2;
goalPos1 = 1000;
goalPos2 = 3000;

IDs = [ID1,ID2,ID3];
GP1 = goalPos1 + [0,0,0];
GP2 = goalPos2 + [0,0,0];

T = 0.5;
pos = NaN(3,10000);
time = NaN(1,10000);
j = 1;
timer = tic;

for i = 1:5
    timer1 = tic;
    setMultiMXposition(IDs,GP1,port);
    while toc(timer1)<T
        pos(:,j) = getMultiMXposition(IDs,port);
        time(j) = toc(timer);
        j = j + 1;
    end

    timer2 = tic;
    setMultiMXposition(IDs,GP2,port);
    while toc(timer2)<T
        pos(:,j) = getMultiMXposition(IDs,port);
        time(j) = toc(timer);
        j = j + 1;
    end
end

figure
plot(time,pos(1,:))
hold on
plot(time,pos(2,:))
plot(time,pos(3,:))

port.delete();
clear port

function success = setMultiMXposition(ID,goalPosition,port)
    %This function is based on Protocol 1.0's "Sync Write" command
    mxGoalPositionAddr = 30;
    toSyncWrite = 131; %0x83

    if length(ID) == length(goalPosition)
        N = length(ID);
    else
        error('ID and goalPosition must have the same length.')
    end

    %Write data to multiple MX simultaneously
    L = 2; %Writing a position takes 2 bytes
    packetLength = ((L + 1) * N) + 4;
    IDbroadcast = 254;

    %Packet length does not include bytes 255, 255, IDbroadcast, packetLength
    %Subtract one from the length because we will add the checksum at the
    %end
    packet = NaN(1,packetLength+4-1); 
    
    headerLength = 7;
    packet(1:headerLength) = [255,255,IDbroadcast,packetLength,toSyncWrite,mxGoalPositionAddr,L];
    
    for i=1:N
        packet(headerLength+3*i-2) = ID(i);

        lowByte = mod(goalPosition(i),256);
        highByte = floor(goalPosition(i)/256);

        packet(headerLength+3*i-1) = lowByte;
        packet(headerLength+3*i) = highByte;
    end
    
    [~,packet(end+1)] = checksumMatch(packet,NaN);
    
    port.write(packet,"uint8");

    success = true;
end

function [positions,success] = getMultiMXposition(ID,port)
    %This function is based on Protocol 1.0's "Bulk Read" command
    mxPresentPositionAddr = 36;
    toBulkRead = 146; %0x92

    N = length(ID);

    %Write data to multiple MX simultaneously
    L = 2; %Reading a position takes 2 bytes
    packetLength = 3*N + 3;
    IDbroadcast = 254;

    %Packet length does not include bytes 255, 255, IDbroadcast, packetLength
    %Subtract one from the length because we will add the checksum at the
    %end
    packet = NaN(1,packetLength+4-1); 
    
    headerLength = 6;
    packet(1:headerLength) = [255,255,IDbroadcast,packetLength,toBulkRead,0];
    
    for i=1:N
        packet(headerLength+3*i-2) = L;
        
        packet(headerLength+3*i-1) = ID(i);

        packet(headerLength+3*i) = mxPresentPositionAddr;
    end
    
    [~,packet(end+1)] = checksumMatch(packet,NaN);
    
    port.write(packet,"uint8");

    while port.NumBytesAvailable < 8*N
        %Do nothing; we just wait for the servo to receive the command to send
        %us data and for the data to be transmitted.
    end

    %Read the available bytes from port's buffer.
    data = port.read(port.NumBytesAvailable,"uint8");

    positions = NaN(N,1);

    for i=1:N
        loopData = data((i-1)*8+(1:8));

        if checksumMatch(loopData,8)

            lowByte = loopData(end-2);
            highByte = loopData(end-1);
            positions(ID == loopData(3)) = highByte*256+lowByte;
        else
            %do nothing, positions will return NaN.
        end
    end

    success = true;
end

function [match,checksum] = checksumMatch(packet,checksumIndex)
    %Provide the packet and which index in the packet is the checksum for
    %comparison.
    %
    %Calculate the checksum. If they match, return true.
    %
    %You can use this function to simply calculate the checksum without
    %comparing it to anything by passing checksumIndex = NaN .
    
    if length(packet) <= 2
        error('Input "packet" must have length of 3 or more.')
    end

    %justCalc is a boolean that determines if we are supposed to just calculate the 
    % checksum value for this packet (true), or if we must also compare
    % the calculated value to the provided packet (false).
    justCalc = (isnan(checksumIndex));

    if justCalc
        preChecksum = sum(packet(3:end));
        preChecksumBinary = int2bit(preChecksum,8);
        checksum = bit2int(~preChecksumBinary,8);
        match = NaN;
    else
        %Save the provided checksum, then remove that index from the
        %packet.
        providedChecksum = packet(checksumIndex);

        packet(checksumIndex) = [];
        preChecksum = sum(packet(3:end));
        preChecksumBinary = int2bit(preChecksum,8);
        checksum = bit2int(~preChecksumBinary,8);

        if providedChecksum == checksum
            match = true;
        else
            match = false;
        end
    end
end