Skip to content

Commit

Permalink
read servo parameters
Browse files Browse the repository at this point in the history
  • Loading branch information
@yasuohayashibara
yasuohayashibara committed Mar 25, 2022
1 parent 3d449b3 commit 9fe75d3
Show file tree
Hide file tree
Showing 3 changed files with 130 additions and 15 deletions.
36 changes: 36 additions & 0 deletions B3MServoChecker/B3MServo.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -278,6 +278,42 @@ public bool setDataLong(byte servoID, byte address, long value)
return true;
}

public bool getDataByte(byte servoID, byte address, ref byte value)
{
ByteList cmd = new ByteList();
byte[] rx = new byte[6];

cmd.Bytes = B3MLib.B3MLib.Read(0x00, address, 1, servoID);
if (B3MLib.B3MLib.Synchronize(_serialPort, cmd.Bytes, ref rx) == false) return false;
value = rx[4];

return true;
}

public bool getDataShort(byte servoID, byte address, ref short value)
{
ByteList cmd = new ByteList();
byte[] rx = new byte[7];

cmd.Bytes = B3MLib.B3MLib.Read(0x00, address, 2, servoID);
if (B3MLib.B3MLib.Synchronize(_serialPort, cmd.Bytes, ref rx) == false) return false;
value = (short)Extensions.Converter.ByteConverter.ByteArrayToInt16(rx[4], rx[5]);

return true;
}

public bool getDataLong(byte servoID, byte address, ref long value)
{
ByteList cmd = new ByteList();
byte[] rx = new byte[9];

cmd.Bytes = B3MLib.B3MLib.Read(0x00, address, 4, servoID);
if (B3MLib.B3MLib.Synchronize(_serialPort, cmd.Bytes, ref rx) == false) return false;
value = (long)Extensions.Converter.ByteConverter.ByteArrayToInt32(rx[4], rx[5], rx[6], rx[7]);

return true;
}

public bool saveROM(byte servoID)
{
ByteList cmd = new ByteList();
Expand Down
14 changes: 14 additions & 0 deletions B3MServoChecker/Form1.Designer.cs
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

95 changes: 80 additions & 15 deletions B3MServoChecker/Form1.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -460,6 +460,71 @@ private void buttonControl270deg_Click(object sender, EventArgs e)
_b3m.setAngle(id, -90);
}

private void buttonGetAllParameters_Click(object sender, EventArgs e)
{
StreamWriter file = new StreamWriter(@"servo_parameters.csv", false, Encoding.UTF8);
file.WriteLine("ID, TYPE, POS_MIN, POS_MAX, POS_CEN, FREQ, DEADBAND, PWM_LIM, GAIN_PRE, KP0, KD0, KI0, STATIC0, DYNAMIC0, KP1, KD1, KI1, STATIC1, DYNAMIC1, KP2, KD2, KI2, STATIC2, DYNAMIC2");
for (byte id = 1; id <= 19; id++)
{
byte motor_type = 0, torque_type = 0;
byte val_byte = 0;
short val_short = 0;
long val_long = 0;
_b3m.getModel(id, ref motor_type, ref torque_type);
file.Write(string.Format("{0}, ", id));
if (motor_type == 'B' && torque_type == 4) file.Write("B3M-SB-1040-A, ");
else if (motor_type == 'C' && torque_type == 4) file.Write("B3M-SC-1040-A, ");
else if (motor_type == 'C' && torque_type == 7) file.Write("B3M-SC-1170-A, ");
_b3m.getDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_MIN, ref val_short);
file.Write(string.Format("{0}, ", val_short));
_b3m.getDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_MAX, ref val_short);
file.Write(string.Format("{0}, ", val_short));
_b3m.getDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_CENTER, ref val_short);
file.Write(string.Format("{0}, ", val_short));
_b3m.getDataShort(id, B3MLib.B3MLib.SERVO_PWM_FREQUENCY, ref val_short);
file.Write(string.Format("{0}, ", val_short));
_b3m.getDataShort(id, B3MLib.B3MLib.SYSTEM_DEADBAND_WIDTH, ref val_short);
file.Write(string.Format("{0}, ", val_short));
_b3m.getDataByte(id, B3MLib.B3MLib.SYSTEM_PWM_LIMIT, ref val_byte);
file.Write(string.Format("{0}, ", val_byte));
_b3m.getDataByte(id, B3MLib.B3MLib.CONTROL_GAIN_PRESET, ref val_byte);
file.Write(string.Format("{0}, ", val_byte));
_b3m.getDataLong(id, B3MLib.B3MLib.CONTROL_KP0, ref val_long);
file.Write(string.Format("{0}, ", val_long));
_b3m.getDataLong(id, B3MLib.B3MLib.CONTROL_KD0, ref val_long);
file.Write(string.Format("{0}, ", val_long));
_b3m.getDataLong(id, B3MLib.B3MLib.CONTROL_KI0, ref val_long);
file.Write(string.Format("{0}, ", val_long));
_b3m.getDataShort(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION0, ref val_short);
file.Write(string.Format("{0}, ", val_short));
_b3m.getDataShort(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION0, ref val_short);
file.Write(string.Format("{0}, ", val_short));
_b3m.getDataLong(id, B3MLib.B3MLib.CONTROL_KP1, ref val_long);
file.Write(string.Format("{0}, ", val_long));
_b3m.getDataLong(id, B3MLib.B3MLib.CONTROL_KD1, ref val_long);
file.Write(string.Format("{0}, ", val_long));
_b3m.getDataLong(id, B3MLib.B3MLib.CONTROL_KI1, ref val_long);
file.Write(string.Format("{0}, ", val_long));
_b3m.getDataShort(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION1, ref val_short);
file.Write(string.Format("{0}, ", val_short));
_b3m.getDataShort(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION1, ref val_short);
file.Write(string.Format("{0}, ", val_short));
_b3m.getDataLong(id, B3MLib.B3MLib.CONTROL_KP2, ref val_long);
file.Write(string.Format("{0}, ", val_long));
_b3m.getDataLong(id, B3MLib.B3MLib.CONTROL_KD2, ref val_long);
file.Write(string.Format("{0}, ", val_long));
_b3m.getDataLong(id, B3MLib.B3MLib.CONTROL_KI2, ref val_long);
file.Write(string.Format("{0}, ", val_long));
_b3m.getDataShort(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION2, ref val_short);
file.Write(string.Format("{0}, ", val_short));
_b3m.getDataShort(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION2, ref val_short);
file.Write(string.Format("{0}, ", val_short));

file.WriteLine("");
}
file.Close();
}

private void buttonSetParameters_Click(object sender, EventArgs e)
{
byte motor_type = 0, torque_type = 0;
Expand All @@ -472,7 +537,7 @@ private void buttonSetParameters_Click(object sender, EventArgs e)
_b3m.setDataLong(id, B3MLib.B3MLib.SYSTEM_BAUDRATE, 1000000);
_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_MIN, -32000);
_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_MAX, 32000);
_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_CENTER, 0);
//_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_CENTER, 0);
_b3m.setDataShort(id, B3MLib.B3MLib.SERVO_PWM_FREQUENCY, 8000);
_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_DEADBAND_WIDTH, 15);
_b3m.setDataByte(id, B3MLib.B3MLib.SYSTEM_PWM_LIMIT, 100);
Expand All @@ -488,26 +553,26 @@ private void buttonSetParameters_Click(object sender, EventArgs e)
_b3m.setDataLong(id, B3MLib.B3MLib.SYSTEM_BAUDRATE, 1000000);
_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_MIN, -32000);
_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_MAX, 32000);
_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_CENTER, 0);
//_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_CENTER, 0);
_b3m.setDataShort(id, B3MLib.B3MLib.SERVO_PWM_FREQUENCY, 8000);
_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_DEADBAND_WIDTH, 15);
_b3m.setDataByte(id, B3MLib.B3MLib.SYSTEM_PWM_LIMIT, 100);
_b3m.setDataByte(id, B3MLib.B3MLib.CONTROL_GAIN_PRESET, 0);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KP0, 35000);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KD0, 500);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KI0, 1800);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION0, 0);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION0, 0);
_b3m.setDataShort(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION0, 0);
_b3m.setDataShort(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION0, 0);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KP1, 5500);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KD1, 100);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KI1, 580);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION1, 100);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION1, 50);
_b3m.setDataShort(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION1, 100);
_b3m.setDataShort(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION1, 50);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KP2, 100000);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KD2, 500);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KI2, 10000);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION2, 100);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION2, 50);
_b3m.setDataShort(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION2, 100);
_b3m.setDataShort(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION2, 50);
_b3m.saveROM(id);
readParameters();
textBoxSetParameterResult.Text = "Success";
Expand All @@ -519,26 +584,26 @@ private void buttonSetParameters_Click(object sender, EventArgs e)
_b3m.setDataLong(id, B3MLib.B3MLib.SYSTEM_BAUDRATE, 1000000);
_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_MIN, -32000);
_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_MAX, 32000);
_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_CENTER, 0);
//_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_POSITION_CENTER, 0);
_b3m.setDataShort(id, B3MLib.B3MLib.SERVO_PWM_FREQUENCY, 8000);
_b3m.setDataShort(id, B3MLib.B3MLib.SYSTEM_DEADBAND_WIDTH, 15);
_b3m.setDataByte(id, B3MLib.B3MLib.SYSTEM_PWM_LIMIT, 100);
_b3m.setDataByte(id, B3MLib.B3MLib.CONTROL_GAIN_PRESET, 0);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KP0, 42000);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KD0, 400);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KI0, 1000);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION0, 0);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION0, 0);
_b3m.setDataShort(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION0, 0);
_b3m.setDataShort(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION0, 0);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KP1, 0);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KD1, 0);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KI1, 0);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION1, 0);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION1, 0);
_b3m.setDataShort(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION1, 0);
_b3m.setDataShort(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION1, 0);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KP2, 42000);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KD2, 750);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_KI2, 6000);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION2, 0);
_b3m.setDataLong(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION2, 50);
_b3m.setDataShort(id, B3MLib.B3MLib.CONTROL_STATIC_FRICTION2, 0);
_b3m.setDataShort(id, B3MLib.B3MLib.CONTROL_DYNAMIC_FRICTION2, 50);
_b3m.saveROM(id);
readParameters();
textBoxSetParameterResult.Text = "Success";
Expand Down

0 comments on commit 9fe75d3

Please sign in to comment.