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