Using the CM5 as translator/compressor

The problem with USB’s inherent latency due to the USB bus controller requiring a cycle of 1ms by definition (maybe USB3 will change this in the future), can be addressed by having long strings sent from the Robotis Dynamixel bus to the PC instead of probing the bus for every servo continuously.

So we will try to use the CM5 as a middle-man. The CM5 is the Bioloid controller board. It will peek/poke the servos continuously and send compressed strings to the PC over USB/FTDI at high speed. Theoretically we should be able to reach 100+ cycles/sec of reading all the servo positions and sending out target positions and torque commands from the PC assuming 1mbps emulated serial port speed.

To get going, we used StuartL’s library and the first piece of code just read the servo positions continuously and printed them out on the terminal.

#include < libavr/flash.h >
#include < bioloid/button.h >
#include < bioloid/dynamixel.h >
#include < bioloid/sio.h >
#include < bioloid/supervisor.h >
#include < bioloid/timer.h >
#include < bioloid/trig.h >

/*

Feb-18 test1.c
this little program continously reads servos positions and prints them out to the terminal.
any attempt to read the values from the "robot" structure failed. it seems that the supervisor
modifies this structure in mysterious ways

having the supervisor is nice becuase when pressing # it goes back to the supervisor..

*/

const robot_t FLASH robot = {
    60, // Temp max
    {
        {  4, MODEL_AX12, SERVO_MIN, SERVO_MAX, TORQUE_MAX },
        {  6, MODEL_AX12, SERVO_MIN, SERVO_MAX, TORQUE_MAX },
        {  11, MODEL_AX12, SERVO_MIN, SERVO_MAX, TORQUE_MAX },
        {  14, MODEL_AX12, SERVO_MIN, SERVO_MAX, TORQUE_MAX },
        {  15, MODEL_AX12, SERVO_MIN, SERVO_MAX, TORQUE_MAX },
        {  17, MODEL_AX12, SERVO_MIN, SERVO_MAX, TORQUE_MAX },
        {  18, MODEL_AX12, SERVO_MIN, SERVO_MAX, TORQUE_MAX },
        {  19, MODEL_AX12, SERVO_MIN, SERVO_MAX, TORQUE_MAX },
        {  40, MODEL_AX12, SERVO_MIN, SERVO_MAX, TORQUE_MAX },
        {  41, MODEL_AX12, SERVO_MIN, SERVO_MAX, TORQUE_MAX },

        { DX_NOID }
    }
};
#define ROBOT_SERVOS (sizeof(robot.device)/sizeof(robot.device[0]))
int myrobot[] =         {  4,6,11,14,15,17,18,19,40,41};

int main(void) {
	static struct dx_stance s = { 0, { [11] = { DX_NOID } } };

    printf("Test1\n");
		for (int i = 0; i < 10; i++) {
			printf("%d ",myrobot[i]);
	}
	printf ("\n");

    for (int i = 0; i < 10; i++) {
		s.servo[i].id = myrobot[i];
        dx_set_torque_enable(myrobot[i], 0);
    }

    while (1) {
		dx_get_stance(&s);
		for (int i = 0; i < 10; i++) {
			printf("%x ", s.servo[i].position);
		}
		putchar('\r');
    }
}