CM-510 as a multiple sensor dynamixel item (updated 7/11/13)

[UPDATE 7/11/13: CM_510 TossModeJCA updated to v5.8. Several parameters error fix and somewhat faster]

My main Bioloid project is to build a robot that will have some interesting (non easily predictable but reasonable) behaviours. AntOne 3.6 is the last version:

AntOne 3.6 is based on a PDA that has the software for the behaviour and a CM-510 (ATMega 2561) that control the hardware (servos and sensors). But I discovered that standard CM-510 firmware has no command to get sensor values, so I programmed another with that function. But then I thought that it would be interesting to embed some perception functions in the CM-510.

This is a general diagrama:

[caption id="attachment_47" align="alignnone" width="547"] Single controller[/caption]

Updated 23/2/2013

The new hex file v4.3 (the old CM-510 firmware)

Now it supports new functions, as:

- Toss Mode (up) now can read from Dynamixel and write to serial and, as the previous version, read from serial and write to Dynamixel, it allows to....
- receive from zigbee and send the data to the Dynamixel button
- read raw sensor values
- averaged sensor values when F_SET_SENSOR_VALUES_TO_FILTER is set < 5, not including the min and the max when >= 5
- CM-510 beep
- sensor scan (still in development)

[sourcecode language="c"]
#define F_NO_FUNCTION 0
#define F_GET_SENSOR_VALUE_RAW 1
#define F_GET_TWO_DMS_SENSOR_VALUES 2
#define F_GET_MULTIPLE_SENSOR_VALUE 3
#define F_GET_SENSOR_VALUE_FILTERED 4
#define F_SET_SENSOR_VALUES_TO_FILTER 5

#define F_BEEP 8
#define F_DO_SENSOR_SCAN 9
#define F_SET_VALUE_DMS1 11
#define F_SET_VALUE_DMS2 12
[/sourcecode]

C# example os use:

[sourcecode language="csharp"]
private void B_ReadPort_Click(object sender, EventArgs e)
{
byte function = (byte)NUD_Function.Value;
byte portId = (byte)NUD_Port.Value;

if (portId < 0 || portId > 6)
portId = 1;

string strPortId=portId.ToString();

if (function == 1 || function == 4)
{
short portValue = cm510.getSensorValue(function, portId);
string str = portValue.ToString();

TB_PortValue.Text = str;
}
else
{
if (function == 2)
{
byte[] parameters = new byte[3];
parameters[0] = function;
parameters[1] = Convert.ToByte(TB_DMS1_ID.Text);
parameters[2] = Convert.ToByte(TB_DMS2_ID.Text);

short[] values = new short[3];
cm510.getMultipleSensorValue(parameters, values);
//cm510.getTwoSensorValue(function, Convert.ToByte(TB_DMS1_ID.Text), Convert.ToByte(TB_DMS2_ID.Text), values);

if (values.Length >= 2)
{
TB_DMS1_PortValue.Text = values[1].ToString();
TB_DMS2_PortValue.Text = values[2].ToString();
}
}
else
{
if (function == 3)
{
byte[] parameters = new byte[3];
parameters[0] = function;
parameters[1] = Convert.ToByte(TB_DMS1_ID.Text);
parameters[2] = Convert.ToByte(TB_DMS2_ID.Text);

short[] values = new short[3];
cm510.getMultipleSensorValue(parameters, values);

if (values.Length >= 2)
{
TB_DMS1_PortValue.Text = values[1].ToString();
TB_DMS2_PortValue.Text = values[2].ToString();
}
}
else
{
if (function == 11)
{
TB_DMS1_ID.Text = strPortId;
}
else
{
if (function == 12)
{
TB_DMS2_ID.Text = strPortId;
}
}
}
}
}
}

[/sourcecode]