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