private void sendValues(float[] armAngles) {
int start = 0;
int end = armAngles.length;
int step = armAngles.length / 2;
while (start < end) {
if (end - start < step) {
step = end - start;
}
byte[] data = new byte[step * 2];
for (int i = 0; i < step; i++) {
int idx = start + i;
float angle =
(float) ArmAngleUtil.radToDig(armAngles[idx]) + mArmSetting.getAngleOffset(idx);
float rate =
(angle - mArmSetting.getServoMin(idx))
/ (mArmSetting.getServoMax(idx) - mArmSetting.getServoMin(idx));
rate = Math.max(Math.min(1, rate), 0);
int value = (int) (0xFFFF * rate);
if (mArmSetting.getServoInvert(idx)) {
value = 0xffff - value;
}
data[i * 2] = (byte) ((value >> 8) & 0xFF);
data[i * 2 + 1] = (byte) (value & 0xFF);
}
try {
if (mAdkService != null) {
mAdkService.sendCommand((byte) 0x03, (byte) (0x01 + (start * 2)), data);
}
} catch (RemoteException e) {
Log.d("Debug", "RemoteException");
// finish();
}
start += step;
}
}
@Override
protected void onCreate(Bundle arg0) {
super.onCreate(arg0);
getWindow().addFlags(WindowManager.LayoutParams.FLAG_FULLSCREEN);
requestWindowFeature(Window.FEATURE_NO_TITLE);
setContentView(R.layout.auto_detection_mode);
findViewById(R.id.grubObjectButton).setOnClickListener(this);
findViewById(R.id.autofocusButton).setOnClickListener(this);
mPositionDetectorFragment =
(PositionDetectorFragment)
getSupportFragmentManager().findFragmentByTag("PositionDetector");
mPositionDetectorFragment.setOnPositionDetectorListener(this);
SharedPreferences pref = PreferenceManager.getDefaultSharedPreferences(this);
mArmSetting.loadPreference(pref);
}
public ErrorType createArmState(List<ArmState> armStates) {
PositionBundle targetBundle = mTargetBundles[0];
if (!mObjectBundle.detected) {
return ErrorType.OBJECT_NOT_DETECTED;
}
if (!targetBundle.detected) {
return ErrorType.TARGET_NOT_DETECTED;
}
float[] lengths =
new float[] {
mArmSetting.getArmLength1(), mArmSetting.getArmLength2(),
mArmSetting.getArmLength3(), mArmSetting.getArmLength4()
};
float[] targetPosition = CtkMath.createVector3f(ANCHOR_POS_OFFSET[0]);
targetPosition[2] += 20;
float[] goodFrontVec = getGoodFrontVec();
if (goodFrontVec == null) {
goodFrontVec = CtkMath.createVector3f(0, 0, -1);
}
ArmState moveToObjectState;
ArmState moveToTargetState;
int index;
{ // MOVE_ABOVE_OBJECT
float[] position =
CtkMath.createVector3f(
-mObjectBundle.position[1],
mObjectBundle.position[0],
mObjectBundle.position[2] + UPPER_MARGIN);
float[] frontVec = CtkMath.createVector3f(0, 0, -1);
float[] upVec = CtkMath.createVector3f(goodFrontVec[1], -goodFrontVec[0], goodFrontVec[2]);
AngleBundle[] angleBundles = ArmAngleUtil.calcAngles(lengths, position, frontVec, upVec);
index = ArmAngleUtil.pickAvailableIndex(angleBundles, mArmSetting);
if (index < 0) {
return ErrorType.BAD_OBJECT_POSITION;
}
float[] armAngles = angleBundles[index].toAnglesFloatArray();
armAngles[5] = (float) ArmAngleUtil.digToRad(mArmSetting.getServo6Min());
armStates.add(new ArmState(ArmStateType.MOVE_ABOVE_OBJECT, armAngles, DELAY_MOVE));
}
{ // MOVE_TO_OBJECT
float[] position =
CtkMath.createVector3f(
-mObjectBundle.position[1], mObjectBundle.position[0], mObjectBundle.position[2]);
float[] frontVec = CtkMath.createVector3f(0, 0, -1);
float[] upVec = CtkMath.createVector3f(goodFrontVec[1], -goodFrontVec[0], goodFrontVec[2]);
AngleBundle[] angleBundles = ArmAngleUtil.calcAngles(lengths, position, frontVec, upVec);
if (!ArmAngleUtil.isAvailableIndex(angleBundles, mArmSetting, index)) {
// 駄目じゃん！
return ErrorType.BAD_OBJECT_POSITION;
}
float[] armAngles = angleBundles[index].toAnglesFloatArray();
armAngles[5] = (float) ArmAngleUtil.digToRad(mArmSetting.getServo6Min());
moveToObjectState = new ArmState(ArmStateType.MOVE_TO_OBJECT, armAngles, DELAY_WAIT);
armStates.add(moveToObjectState);
}
{ // GRAB_OBJECT
ArmState armState = new ArmState(moveToObjectState);
armState.armStateType = ArmStateType.GRAB_OBJECT;
armState.armAngles[5] = (float) ArmAngleUtil.digToRad(mArmSetting.getServo6Max() / 3);
armState.time = DELAY_WAIT;
armStates.add(armState);
}
int tIndex = -1;
{ // MOVE_ABOVE_TARGET
float[] position =
CtkMath.createVector3f(
-targetBundle.position[1],
targetBundle.position[0],
targetBundle.position[2] + UPPER_MARGIN + OBJECT_SIZE / 2);
float[] frontVec = CtkMath.createVector3f(0, 0, -1);
float[] upVec = CtkMath.createVector3f(1, 0, 0);
AngleBundle[] angleBundles = ArmAngleUtil.calcAngles(lengths, position, frontVec, upVec);
tIndex = ArmAngleUtil.pickAvailableIndex(angleBundles, mArmSetting);
if (tIndex == -1) {
// 駄目じゃん！
return ErrorType.BAD_TARGET_POSITION;
}
float[] armAngles = angleBundles[tIndex].toAnglesFloatArray();
armAngles[5] = (float) ArmAngleUtil.digToRad(mArmSetting.getServo6Max() / 3);
moveToTargetState = new ArmState(ArmStateType.MOVE_ABOVE_TARGET, armAngles, DELAY_MOVE);
armStates.add(moveToTargetState);
}
{ // MOVE_TO_TARGET
float[] position =
CtkMath.createVector3f(
-targetBundle.position[1],
targetBundle.position[0],
targetBundle.position[2] + OBJECT_SIZE / 2 + EXTRA_MARGIN_RELEASE);
float[] frontVec = CtkMath.createVector3f(0, 0, -1);
float[] upVec = CtkMath.createVector3f(1, 0, 0);
AngleBundle[] angleBundles = ArmAngleUtil.calcAngles(lengths, position, frontVec, upVec);
tIndex = ArmAngleUtil.pickAvailableIndex(angleBundles, mArmSetting);
if (!ArmAngleUtil.isAvailableIndex(angleBundles, mArmSetting, tIndex)) {
// 駄目じゃん！
return ErrorType.BAD_TARGET_POSITION;
}
float[] armAngles = angleBundles[tIndex].toAnglesFloatArray();
armAngles[5] = (float) ArmAngleUtil.digToRad(mArmSetting.getServo6Max() / 3);
moveToTargetState = new ArmState(ArmStateType.MOVE_TO_TARGET, armAngles, DELAY_WAIT);
armStates.add(moveToTargetState);
}
{ // RELEASE_OBJECT
ArmState armState = new ArmState(moveToTargetState);
armState.armStateType = ArmStateType.RELEASE_OBJECT;
armState.armAngles[5] = (float) ArmAngleUtil.digToRad(mArmSetting.getServo6Min());
armState.time = DELAY_WAIT;
armStates.add(armState);
}
{ // MOVE_TO_RELEASED_OBJECT
float[] position =
CtkMath.createVector3f(
-targetBundle.position[1],
targetBundle.position[0],
targetBundle.position[2] + UPPER_MARGIN + OBJECT_SIZE / 2);
float[] frontVec = CtkMath.createVector3f(0, 0, -1);
float[] upVec = CtkMath.createVector3f(1, 0, 0);
AngleBundle[] angleBundles = ArmAngleUtil.calcAngles(lengths, position, frontVec, upVec);
if (!ArmAngleUtil.isAvailableIndex(angleBundles, mArmSetting, tIndex)) {
// 駄目じゃん！
return ErrorType.BAD_TARGET_POSITION;
}
float[] armAngles = angleBundles[tIndex].toAnglesFloatArray();
armAngles[5] = (float) ArmAngleUtil.digToRad(mArmSetting.getServo6Min());
armStates.add(new ArmState(ArmStateType.MOVE_ABOVE_RELEASED_OBJECT, armAngles, DELAY_WAIT));
}
return ErrorType.NO_ERROR;
}
