private void SetMotorConfiguration(byte motorID, MotorConfiguration mc)
{
if (GetMotorIDs().Contains(motorID))
{
servo.SetDegree(motorID, mc.GetShortValue("Degree_Min"), mc.GetShortValue("Degree_Max"));
servo.SetPulseWidth(motorID, mc.GetIntValue("PulseWidth_Min"), mc.GetIntValue("PulseWidth_Max"));
servo.SetPeriod(motorID, mc.GetIntValue("Period"));
servo.SetAcceleration(motorID, mc.GetIntValue("Acceleration"));
servo.SetVelocity(motorID, mc.GetIntValue("Velocity"));
servo.SetPosition(motorID, mc.GetShortValue("Position"));
servo.Enable(motorID);
}
else
{
throw new Exception("Unkown motorID in SetMotorConfiguration: " + motorID);
}
}
public TFRoboticArmWith6DOF()
{
// Connection to brickd
ipcon = new IPConnection();
servo = new BrickServo(UID, ipcon);
ipcon.Connect(HOST, PORT);
servo.SetOutputVoltage(5000); // 5V
MotorConfiguration mcServoTypeA = new MotorConfiguration();
mcServoTypeA.SetValue("Degree_Min", "-6700");
mcServoTypeA.SetValue("Degree_Max", "6700");
mcServoTypeA.SetValue("PulseWidth_Min", "800");
mcServoTypeA.SetValue("PulseWidth_Max", "2700");
mcServoTypeA.SetValue("Period", "19500");
mcServoTypeA.SetValue("Acceleration", "65535");
mcServoTypeA.SetValue("Velocity", "65535");
mcServoTypeA.SetValue("Position", "0");
MotorConfiguration mcServoTypeB = new MotorConfiguration();
mcServoTypeB.SetValue("Degree_Min", "-6700");
mcServoTypeB.SetValue("Degree_Max", "6700");
mcServoTypeB.SetValue("PulseWidth_Min", "800");
mcServoTypeB.SetValue("PulseWidth_Max", "2100");
mcServoTypeB.SetValue("Period", "19500");
mcServoTypeB.SetValue("Acceleration", "65535");
mcServoTypeB.SetValue("Velocity", "65535");
mcServoTypeB.SetValue("Position", "0");
SetMotorConfiguration(JOINT1_SERVO, mcServoTypeA);
SetMotorConfiguration(JOINT2_SERVO, mcServoTypeA);
SetMotorConfiguration(JOINT3_SERVO, mcServoTypeB);
SetMotorConfiguration(JOINT4_SERVO, mcServoTypeB);
SetMotorConfiguration(JOINT5_SERVO, mcServoTypeB);
SetMotorConfiguration(JOINT6_SERVO, mcServoTypeB);
}
