/// <summary> /// Release servo. Release a servo from holding its position. /// If modified, stops the servo. /// </summary> public async Task ReleaseServo(ServoPortEnum servoPort) { _servoLastMoveTime[(int)servoPort] = DateTime.Now; _servoReleased[(int)servoPort] = true; if (servoPort >= ServoPortEnum.D0 && servoPort <= ServoPortEnum.D23) { await _ezb.sendCommand(EZB.CommandEnum.CmdSetServoPosition + (byte)servoPort, new byte[] { (byte)0 }); } else if (servoPort >= ServoPortEnum.AX0 && servoPort <= ServoPortEnum.AX50 && _ezb.EZBType == EZB.EZ_B_Type_Enum.ezb4) { byte id = (byte)(servoPort - ServoPortEnum.AX0); List <byte> cmdData = new List <byte>(); cmdData.Add((byte)EZB.CommandEnum.CmdEZBv4); cmdData.Add((byte)EZB.CmdEZBv4Enum.CmdV4SetAX12Servo); byte[] cmd = Dynamixel.ReleaseServo(id); cmdData.Add((byte)cmd.Length); cmdData.AddRange(cmd); await _ezb.sendCommandData(0, cmdData.ToArray()); } else if (servoPort >= ServoPortEnum.AXV0 && servoPort <= ServoPortEnum.AXV50 && _ezb.EZBType == EZB.EZ_B_Type_Enum.ezb4) { byte id = (byte)(servoPort - ServoPortEnum.AXV0); List <byte> cmdData = new List <byte>(); cmdData.Add((byte)EZB.CommandEnum.CmdEZBv4); cmdData.Add((byte)EZB.CmdEZBv4Enum.CmdV4SetAX12Servo); byte[] cmd = DynamixelV2.ReleaseServo(id); cmdData.Add((byte)cmd.Length); cmdData.AddRange(cmd); await _ezb.sendCommandData(0, cmdData.ToArray()); } }
/// <summary> /// Set the speed of multiple servos /// </summary> /// <param name="servoPort"></param> /// <param name="speed"></param> public async Task SetServoSpeed(ServoPortEnum[] servoPorts, int speed) { if (!_ezb.IsConnected) { return; } if (speed > SERVO_SPEED_SLOWEST) { speed = SERVO_SPEED_SLOWEST; } else if (speed < SERVO_SPEED_FASTEST) { speed = SERVO_SPEED_FASTEST; } List <byte> cmdData = new List <byte>(); foreach (ServoPortEnum servoPort in servoPorts) { if (_servoReleased[(int)servoPort]) { return; } if (_servoSpeeds[(int)servoPort] == speed) { return; } _servoSpeeds[(int)servoPort] = speed; if (servoPort >= ServoPortEnum.D0 && servoPort <= ServoPortEnum.D23) { cmdData.Add((byte)((byte)EZB.CommandEnum.CmdSetServoSpeed + servoPort)); cmdData.Add((byte)speed); } else if (servoPort >= ServoPortEnum.AX0 && servoPort <= ServoPortEnum.AX50 && _ezb.EZBType == EZB.EZ_B_Type_Enum.ezb4) { byte id = (byte)(servoPort - ServoPortEnum.AX0); int position = speed * 51; cmdData.Add((byte)EZB.CommandEnum.CmdEZBv4); cmdData.Add((byte)EZB.CmdEZBv4Enum.CmdV4SetAX12Servo); byte[] cmd = Dynamixel.ServoSpeed(id, position); cmdData.Add((byte)cmd.Length); cmdData.AddRange(cmd); } else if (servoPort >= ServoPortEnum.AXV0 && servoPort <= ServoPortEnum.AXV50 && _ezb.EZBType == EZB.EZ_B_Type_Enum.ezb4) { byte id = (byte)(servoPort - ServoPortEnum.AXV0); int position = speed * 51; cmdData.Add((byte)EZB.CommandEnum.CmdEZBv4); cmdData.Add((byte)EZB.CmdEZBv4Enum.CmdV4SetAX12Servo); byte[] cmd = DynamixelV2.ServoSpeed(id, position); cmdData.Add((byte)cmd.Length); cmdData.AddRange(cmd); } } await _ezb.sendCommandData(0, cmdData.ToArray()); }
/// <summary> /// Set the position of a servo /// Uses the last speed specified /// </summary> public async Task SetServoPosition(Classes.ServoItem[] servos) { if (!_ezb.IsConnected) { return; } List <byte> cmdData = new List <byte>(); foreach (Classes.ServoItem servo in servos) { if (servo.Position < _servoMin[(int)servo.Port] || servo.Position > _servoMax[(int)servo.Port]) { continue; } _servoPositions[(int)servo.Port] = servo.Position; _servoReleased[(int)servo.Port] = false; _servoLastMoveTime[(int)servo.Port] = DateTime.Now; int tmpPosition = servo.Position + _servoFineTune[(int)servo.Port]; if (tmpPosition > SERVO_MAX) { tmpPosition = SERVO_MAX; } else if (tmpPosition < SERVO_MIN) { tmpPosition = SERVO_MIN; } if (servo.Port >= ServoPortEnum.D0 && servo.Port <= ServoPortEnum.D23) { cmdData.AddRange(new byte[] { (byte)(EZB.CommandEnum.CmdSetServoPosition + (byte)servo.Port), (byte)tmpPosition }); } else if (servo.Port >= ServoPortEnum.AX0 && servo.Port <= ServoPortEnum.AX50 && _ezb.EZBType == EZB.EZ_B_Type_Enum.ezb4) { byte id = (byte)(servo.Port - ServoPortEnum.AX0); int position = (int)(tmpPosition * 5.689); cmdData.Add((byte)EZB.CommandEnum.CmdEZBv4); cmdData.Add((byte)EZB.CmdEZBv4Enum.CmdV4SetAX12Servo); byte[] cmd1 = Dynamixel.GetDisableStatusPacketCmd(id); byte[] cmd2 = Dynamixel.MoveServoCmd(id, position); cmdData.Add((byte)(cmd1.Length + cmd2.Length)); cmdData.AddRange(cmd1); cmdData.AddRange(cmd2); } else if (servo.Port >= ServoPortEnum.AXV0 && servo.Port <= ServoPortEnum.AXV50 && _ezb.EZBType == EZB.EZ_B_Type_Enum.ezb4) { List <byte> tmpData = new List <byte>(); byte id = (byte)(servo.Port - ServoPortEnum.AXV0); int position = (int)(tmpPosition * 5.689); tmpData.Add((byte)EZB.CommandEnum.CmdEZBv4); tmpData.Add((byte)EZB.CmdEZBv4Enum.CmdV4SetAX12Servo); byte[] cmd1 = DynamixelV2.MoveServoCmd(id, position); byte[] cmd2 = DynamixelV2.GetDisableStatusPacketCmd(id); tmpData.Add((byte)(cmd1.Length + cmd2.Length)); tmpData.AddRange(cmd1); tmpData.AddRange(cmd2); await _ezb.sendCommandData(0, tmpData.ToArray()); } } if (cmdData.Count > 0) { await _ezb.sendCommandData(0, cmdData.ToArray()); } if (OnServoMove != null) { OnServoMove(servos); } }