243 lines
9.1 KiB
Java
243 lines
9.1 KiB
Java
package com.ubtrobot.jimu.robotapi;
|
|
|
|
import android.util.Log;
|
|
import com.ubtrobot.jimu.bluetooth.base.IPacket;
|
|
import com.ubtrobot.jimu.bluetooth.base.ProtocolPacket;
|
|
import com.ubtrobot.jimu.connection.RequestException;
|
|
import com.ubtrobot.jimu.connection.ResponseCallback;
|
|
import com.ubtrobot.jimu.connection.RobotConnection;
|
|
import com.ubtrobot.log.ALog;
|
|
import java.util.ArrayList;
|
|
import java.util.Arrays;
|
|
import java.util.List;
|
|
|
|
/* loaded from: classes2.dex */
|
|
public class ServoManager extends MotorManager {
|
|
public ServoManager(RobotConnection robotConnection) {
|
|
super(robotConnection);
|
|
}
|
|
|
|
public void a(int[] iArr, float[] fArr, int i, int i2) throws MotorException {
|
|
int length = fArr.length;
|
|
byte[] bArr = new byte[length + 4 + 1 + 2];
|
|
byte[] a = a(iArr);
|
|
byte[] bArr2 = new byte[length];
|
|
for (int i3 = 0; i3 < length; i3++) {
|
|
bArr2[i3] = (byte) fArr[i3];
|
|
}
|
|
System.arraycopy(a, 0, bArr, 0, a.length);
|
|
int length2 = a.length + 0;
|
|
System.arraycopy(bArr2, 0, bArr, length2, bArr2.length);
|
|
int length3 = length2 + bArr2.length;
|
|
bArr[length3] = (byte) (i / 20);
|
|
int i4 = length3 + 1;
|
|
int i5 = i + i2;
|
|
bArr[i4] = (byte) ((65280 & i5) >> 8);
|
|
bArr[i4 + 1] = (byte) (i5 & 255);
|
|
try {
|
|
byte[] g = this.a.a(new ProtocolPacket(9, bArr)).g();
|
|
if (g.length == 1 && g[0] == 0) {
|
|
return;
|
|
}
|
|
if (g.length != 5) {
|
|
throw new MotorException(-21, "cmd result is fail! ErrCode:" + ((int) g[0]));
|
|
}
|
|
throw new MotorException(-21, "cmd result is fail! ErrCode:" + ((int) g[0]), a(Arrays.copyOfRange(g, 1, g.length)));
|
|
} catch (RequestException e) {
|
|
throw new MotorException(e.getCode(), e.getMessage());
|
|
}
|
|
}
|
|
|
|
public void b() throws JimuException {
|
|
b(0, false);
|
|
}
|
|
|
|
public ServoAngleReadInfo b(int i, boolean z) throws JimuException {
|
|
new ArrayList();
|
|
byte[] bArr = new byte[2];
|
|
bArr[0] = (byte) i;
|
|
boolean z2 = true;
|
|
if (z) {
|
|
bArr[1] = 1;
|
|
} else {
|
|
bArr[1] = 0;
|
|
}
|
|
ProtocolPacket protocolPacket = new ProtocolPacket(11, bArr);
|
|
try {
|
|
byte[] g = this.a.a(protocolPacket).g();
|
|
if (g.length == 1 || g.length != 6) {
|
|
throw new JimuException(-22, "Control fail!");
|
|
}
|
|
int i2 = g[0] & 255;
|
|
if ((g[1] & 255) != 170) {
|
|
z2 = false;
|
|
}
|
|
return new ServoAngleReadInfo(i2, z2, ((g[2] & 255) << 8) | (g[3] & 255), (g[5] & 255) | ((g[4] & 255) << 8));
|
|
} catch (RequestException e) {
|
|
Log.e("ServoManager", "Call cmd fail! cmd:" + ((int) protocolPacket.f()), e);
|
|
throw new JimuException(e.getCode(), e.getMessage());
|
|
}
|
|
}
|
|
|
|
public void a(int[] iArr, int i) throws MotorException {
|
|
int length = iArr.length;
|
|
byte[] bArr = new byte[length + 4];
|
|
bArr[0] = (byte) (length & 255);
|
|
int i2 = 0;
|
|
while (i2 < length) {
|
|
int i3 = i2 + 1;
|
|
bArr[i3] = (byte) iArr[i2];
|
|
i2 = i3;
|
|
}
|
|
if (i > 0) {
|
|
bArr[length + 1] = 1;
|
|
} else if (i < 0) {
|
|
bArr[length + 1] = 2;
|
|
} else {
|
|
bArr[length + 1] = 0;
|
|
}
|
|
int abs = Math.abs(i);
|
|
bArr[length + 2] = (byte) ((65280 & abs) >> 8);
|
|
bArr[length + 3] = (byte) (abs & 255);
|
|
try {
|
|
byte[] g = this.a.a(new ProtocolPacket(7, bArr)).g();
|
|
if (g.length == 1 && g[0] == 0) {
|
|
return;
|
|
}
|
|
if (g.length == 5) {
|
|
throw new MotorException(-21, "cmd result is fail! ErrCode:" + ((int) g[0]), a(Arrays.copyOfRange(g, 1, g.length)));
|
|
}
|
|
throw new MotorException(-21, "cmd result is fail! ErrCode:" + ((int) g[0]));
|
|
} catch (RequestException e) {
|
|
throw new MotorException(e.getCode(), e.getMessage());
|
|
}
|
|
}
|
|
|
|
@Override // com.ubtrobot.jimu.robotapi.MotorManager
|
|
public void a() throws JimuException {
|
|
try {
|
|
byte[] g = this.a.a(new ProtocolPacket(59, new byte[0])).g();
|
|
if (g[0] == 0) {
|
|
return;
|
|
}
|
|
throw new JimuException(-21, "cmd result is fail! ErrCode:" + ((int) g[0]));
|
|
} catch (RequestException e) {
|
|
throw new JimuException(e.getCode(), e.getMessage());
|
|
}
|
|
}
|
|
|
|
public List<ServoAngleReadInfo> a(final int i, boolean z) throws JimuException {
|
|
final boolean[] zArr = new boolean[1];
|
|
final boolean[] zArr2 = new boolean[1];
|
|
final JimuException[] jimuExceptionArr = new JimuException[1];
|
|
final ArrayList arrayList = new ArrayList();
|
|
byte[] bArr = new byte[2];
|
|
bArr[0] = 0;
|
|
if (z) {
|
|
bArr[1] = 1;
|
|
} else {
|
|
bArr[1] = 0;
|
|
}
|
|
ProtocolPacket protocolPacket = new ProtocolPacket(11, bArr);
|
|
this.a.a(protocolPacket, new ResponseCallback(this) { // from class: com.ubtrobot.jimu.robotapi.ServoManager.1
|
|
@Override // com.ubtrobot.jimu.connection.ResponseCallback
|
|
public void a(IPacket iPacket, IPacket iPacket2) {
|
|
byte[] g = iPacket2.g();
|
|
if (g.length == 1 || g.length != 6) {
|
|
synchronized (zArr) {
|
|
jimuExceptionArr[0] = new JimuException(-22, "Control fail!");
|
|
zArr[0] = true;
|
|
ALog.a("ServoManager").d("responded notify");
|
|
zArr.notifyAll();
|
|
}
|
|
return;
|
|
}
|
|
ServoAngleReadInfo servoAngleReadInfo = new ServoAngleReadInfo(g[0] & 255, (g[1] & 255) == 170, ((g[2] & 255) << 8) | (g[3] & 255), (g[5] & 255) | ((g[4] & 255) << 8));
|
|
synchronized (zArr) {
|
|
arrayList.add(servoAngleReadInfo);
|
|
if (arrayList.size() == i) {
|
|
zArr2[0] = true;
|
|
zArr[0] = true;
|
|
Log.d("ServoManager", "responded notify");
|
|
zArr.notifyAll();
|
|
}
|
|
}
|
|
}
|
|
|
|
@Override // com.ubtrobot.jimu.connection.ResponseCallback
|
|
public void a(IPacket iPacket, RequestException requestException) {
|
|
synchronized (zArr) {
|
|
jimuExceptionArr[0] = new JimuException(requestException.getCode(), requestException.getMessage());
|
|
zArr[0] = true;
|
|
Log.d("ServoManager", "responded notify");
|
|
zArr.notifyAll();
|
|
}
|
|
}
|
|
}, i);
|
|
synchronized (zArr) {
|
|
while (!zArr[0]) {
|
|
try {
|
|
Log.d("ServoManager", "responded wait");
|
|
zArr.wait();
|
|
break;
|
|
} catch (InterruptedException e) {
|
|
Log.w("ServoManager", "Interrupted by someone when sync doRequest. req=" + protocolPacket + "Ignore it.", e);
|
|
Thread.currentThread().interrupt();
|
|
}
|
|
}
|
|
if (jimuExceptionArr[0] != null) {
|
|
throw jimuExceptionArr[0];
|
|
}
|
|
}
|
|
return arrayList;
|
|
}
|
|
|
|
@Override // com.ubtrobot.jimu.robotapi.MotorManager
|
|
public int a(int i, int i2) throws JimuException {
|
|
ProtocolPacket protocolPacket = new ProtocolPacket(12, new byte[]{(byte) i, (byte) i2});
|
|
try {
|
|
byte[] g = this.a.a(protocolPacket).g();
|
|
if (g.length == 1) {
|
|
return g[0] & 255;
|
|
}
|
|
throw new JimuException(-22, "res argument is err!");
|
|
} catch (RequestException e) {
|
|
Log.e("ServoManager", "Call cmd fail! cmd:" + ((int) protocolPacket.f()), e);
|
|
throw new JimuException(e.getCode(), e.getMessage());
|
|
}
|
|
}
|
|
|
|
private byte[] a(int[] iArr) {
|
|
byte[] bArr = new byte[4];
|
|
for (int i : iArr) {
|
|
if (i > 0 && i <= 8) {
|
|
bArr[3] = (byte) (bArr[3] + (1 << (i - 1)));
|
|
} else if (i > 8 && i <= 16) {
|
|
bArr[2] = (byte) (bArr[2] + (1 << ((i - 1) - 8)));
|
|
} else if (i > 16 && i <= 24) {
|
|
bArr[1] = (byte) (bArr[1] + (1 << ((i - 1) - 16)));
|
|
} else if (i >= 24 && i <= 32) {
|
|
bArr[0] = (byte) (bArr[0] + (1 << ((i - 1) - 24)));
|
|
}
|
|
}
|
|
return bArr;
|
|
}
|
|
|
|
private ArrayList<Integer> a(byte[] bArr) {
|
|
ArrayList<Integer> arrayList = new ArrayList<>();
|
|
if (bArr.length != 4) {
|
|
return arrayList;
|
|
}
|
|
for (int i = 0; i < bArr.length; i++) {
|
|
int i2 = 24 - (i * 8);
|
|
byte b = bArr[i];
|
|
for (int i3 = 0; i3 < 8; i3++) {
|
|
if ((b & 255 & (1 << i3)) != 0) {
|
|
arrayList.add(Integer.valueOf(i3 + 1 + i2));
|
|
}
|
|
}
|
|
}
|
|
return arrayList;
|
|
}
|
|
}
|