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); }