void checkForCompletedAnchors() { // If any anchors have reached their drop points, then trigger a restriction zone rebuild int readyForWorkIdleCheckCount = 0; if (anchorPoints != null) { foreach (BMod_AnchorPoint anchorPoint in anchorPoints) { if (anchorPoint.completedLockingSignal) { restrictedMovementAreaUpdateRequired = true; anchorPoint.completedLockingSignal = false; // Reset the signal } if (balPosWorkingStatus == balPolStatusEnum.startingUp) { // Part of start up sequence. Due to the rigid frame not being in a suitable // position on loading, everything must wait until the anchorpoints have // reached their lock positions before anything is sent to the linked frame. if (anchorPoint.currentAction == BMod_AnchorPoint.AnchorCurrentActionEnum.idle) { readyForWorkIdleCheckCount++; } } } // If all the anchor points are idle, then the startup sequence is complete. if (balPosWorkingStatus == balPolStatusEnum.startingUp) { if (readyForWorkIdleCheckCount == anchorPoints.Length) { balPosWorkingStatus = balPolStatusEnum.readyForWork; } } } }
// Update is called once per frame void Update() { // TODO Balance position needs to be upgraded to use a centre of balance // system, where the body is tilted and shifted as needed. // Process each of the distance holder and anchor points attached to this // class. if (restrictedMovementAreaUpdateRequired) { updateRestrictionRegion(false); restrictedMovementAreaUpdateRequired = false; } // Control pad movement float hozAxis = Input.GetAxis("Horizontal") * maxSlideMovementSpeed; float verAxis = Input.GetAxis("Vertical") * maxSlideMovementSpeed; noncumulativeOffset.x = Input.GetAxis("Horizontal"); noncumulativeOffset.z = Input.GetAxis("Vertical"); float keyBiasOffset = 0.5f; // This goes towards how far forward the anchor point // is positioned in the direction of movement for keyboard based control. // Collect bias from external controls if (Input.GetKey(KeyCode.UpArrow)) { slideOffset.z = slideOffset.z + (maxSlideMovementSpeed * Time.deltaTime); noncumulativeOffset.z = keyBiasOffset; noncumulativeOffset.x = 0.0f; } if (Input.GetKey(KeyCode.DownArrow)) { slideOffset.z = slideOffset.z - (maxSlideMovementSpeed * Time.deltaTime); noncumulativeOffset.z = -keyBiasOffset; noncumulativeOffset.x = 0.0f; } if (Input.GetKey(KeyCode.LeftArrow)) { slideOffset.x = slideOffset.x + (maxSlideMovementSpeed * Time.deltaTime); noncumulativeOffset.z = 0.0f; noncumulativeOffset.x = keyBiasOffset; } if (Input.GetKey(KeyCode.RightArrow)) { slideOffset.x = slideOffset.x - (maxSlideMovementSpeed * Time.deltaTime); noncumulativeOffset.z = 0.0f; noncumulativeOffset.x = -keyBiasOffset; } // Controls switches bool isLeaningSwitch = Input.GetButton("Button_R"); bool isNoLegLiftingSwitch = Input.GetButton("Button_L"); // Manual leg lifting button presses int anchorLift = -1; if (Input.GetButtonDown("Button_A")) { anchorLift = 0; } if (Input.GetButtonDown("Button_B")) { anchorLift = 1; } if (Input.GetButtonDown("Button_X")) { anchorLift = 3; } if (Input.GetButtonDown("Button_Y")) { anchorLift = 2; } if (anchorLift != -1) { BMod_AnchorPoint anchorPoint = anchorPoints[anchorLift]; anchorPoint.needsToLift = true; } //Debug.Log("Hoz " + hozAxis + ", Ver " + verAxis); // Only apply horizontal and vertical slide axis if bal pos is ready for work if (balPosWorkingStatus == balPolStatusEnum.readyForWork) { slideOffset.x = slideOffset.x + (hozAxis * Time.deltaTime); slideOffset.z = slideOffset.z + (verAxis * Time.deltaTime); } // Rotation around centre point slide calculations here float rotAxis = Input.GetAxis("Hoz Rotate") * rotationDegreeDelta; // Calculate the restriction centre and compare with the actual roving centre. Vector3 biasActualCentre = new Vector3(); foreach (Vector3 biasVector in biasMovementRestrictionArea) { biasActualCentre = biasActualCentre + biasVector; } biasActualCentre = biasActualCentre / biasMovementRestrictionArea.Count; if (Vector3.Distance(biasCentreTrack, biasActualCentre) > 0.0f) { // If the centre if off then the robot is adjusting its balance balPosWorkingStatus = balPolStatusEnum.adjustingBalance; Vector3 biasMoved = Vector3.MoveTowards(biasCentreTrack, biasActualCentre, maxSlideMovementSpeed * Time.deltaTime); slideOffset = slideOffset + (biasCentreTrack - biasMoved) * 1.2f; // Boost the movement // Move the bias centre. biasCentreTrack = biasCentreTrack - (biasCentreTrack - biasMoved); if (biasCentreRepresentation != null) { biasCentreRepresentation.transform.position = biasCentreTrack; } } else { // If the balance point has been reached then declare ready for work, unless still in the start up sequence. if (balPosWorkingStatus != balPolStatusEnum.startingUp) { balPosWorkingStatus = balPolStatusEnum.readyForWork; } } // Roll through the anchor points and collect the limit reached offset vectices foreach (BMod_AnchorPoint anchorPoint in anchorPoints) { // Only pull from the ones locked in place if (anchorPoint.lockedInPosition) { Vector3 limitCorrection = anchorPoint.limitReachCorrectionVector(); slideOffset.x = slideOffset.x + limitCorrection.x; slideOffset.z = slideOffset.z + limitCorrection.z; } } // Update position sliders (Anchor points for now) with acceleration bias. // Only allow movement when balanced. if ((previousSlideOffset != slideOffset || rotAxis != 0.0f) && !isLeaningSwitch && anchorPoints != null) { // Apply the slide and rotation changes to the anchor points. float angleRad = rotAxis * rotationDegreeDelta * Mathf.Deg2Rad; // Changed rotationDegreeDelta to movementSpeed foreach (BMod_AnchorPoint anchorPoint in anchorPoints) { if (anchorPoint.lockedInPosition) { // Only apply offset as translation difference from previous offset if the anchor is locked. Vector3 rotationOffset = Vector3.zero; if (rotAxis != 0.0f) { // Rotation offset Vector3 currentPosition = anchorPoint.anchorVertex.transform.position; // The current position should be a vector from x = 0, z = 0. So be applying a rotation // to the current position and keeping the offset, this should work as a way of creating rotation. float xVal = currentPosition.x * Mathf.Cos(angleRad) - currentPosition.z * Mathf.Sin(angleRad); float yVal = currentPosition.y; // We're not changing the height. Future versions may need to height check. float zVal = currentPosition.x * Mathf.Sin(angleRad) + currentPosition.z * Mathf.Cos(angleRad); rotationOffset = (new Vector3(xVal, yVal, zVal)) - currentPosition; } anchorPoint.anchorVertex.offset += (slideOffset - previousSlideOffset) + rotationOffset; // Apply the change in offset } } previousSlideOffset = slideOffset; } // Check if any anchors have reached thier drop points. checkForCompletedAnchors(); // If the counter bias has zero movement then any anchor points waiting for // release can be processed. // Check if prevent lift button is not pressed if (!isNoLegLiftingSwitch) { checkForReleaseableAnchors(); } //else { // Debug.Log("Button R pressed held"); // } // Adjust distance holder offsets float crouchAxis = Input.GetAxis("Crouch") * crouchRange; if (distanceHolders != null) { if (isLeaningSwitch) { // This is to allow the rotational leaning of the robot. Vector3 leanAxis = new Vector3(-Input.GetAxis("Hoz Rotate"), 0.0f, Input.GetAxis("Crouch")); leanAxis = leanAxis * 1.5f; // Increase the movement range foreach (BMod_DistanceHolder distanceHolder in distanceHolders) { Vector3 distHold = distanceHolder.vertexPoint.transform.position; Vector3 groundHolder = new Vector3(distHold.x, 0.0f, distHold.z); float distToCentre = Vector3.Distance(biasActualCentre, groundHolder); float distToTracked = Vector3.Distance(biasActualCentre + leanAxis, groundHolder); float ratioAlt = distToTracked / distToCentre; float tiltOffset = 0.0f; if (ratioAlt != 1.0) { tiltOffset = distanceHolder.distanceToHold - (distanceHolder.distanceToHold * ratioAlt); } distanceHolder.distanceToHoldOffset = tiltOffset; } } else { // Alter the height of each distance holder by the crouch axis foreach (BMod_DistanceHolder distanceHolder in distanceHolders) { distanceHolder.distanceToHoldOffset = crouchAxis; } } } }
void checkForReleaseableAnchors() { // Cycle through anchor points to check for any that need to be released. // Priority given to those furthest away from this balance position, which any tied then on priority value. if (anchorPoints != null) { BMod_AnchorPoint anchorToUnlock = null; int unlockedCount = 0; List <BMod_AnchorPoint> requestors = new List <BMod_AnchorPoint>(); foreach (BMod_AnchorPoint anchor in anchorPoints) { if (anchor.needsToLift) { requestors.Add(anchor); } if (anchor.lockedInPosition == false || anchor.currentAction == BMod_AnchorPoint.AnchorCurrentActionEnum.waitingForBalance) { unlockedCount++; } } // Only proceed if there's less anchor points unlocked than the maximum allowed. if (unlockedCount < maxReleasable && requestors.Count > 0) { if (requestors.Count > maxReleasable) { // Filter by distance and then by priority if needed. // Distance is how far from original lock position // Use a dictionary with the distance rounded to whole number. If a duplicat entry exists, // it is overriden if the duplcate has a lower priority level. // Dictionary is then sorted highest to lowest and first entry taken out. Dictionary <int, BMod_AnchorPoint> distanceAnchors = new Dictionary <int, BMod_AnchorPoint>(); foreach (BMod_AnchorPoint anchorCheck in requestors) { // Only working with ground height of zero for the distances for now. // Was used originally as part of the pushing through the ground test. Vector3 nonHeightAnchor = new Vector3(anchorCheck.anchorVertex.transform.position.x, 0.0f, anchorCheck.anchorVertex.transform.position.z); int distanceToOriginalLockPoint = Mathf.RoundToInt(Vector3.Distance(anchorCheck.lockedPosition, nonHeightAnchor) * 10.0f); if (distanceAnchors.ContainsKey(distanceToOriginalLockPoint)) { if (distanceAnchors[distanceToOriginalLockPoint].priorityIndex < anchorCheck.priorityIndex) { // Replace original anchor point distanceAnchors[distanceToOriginalLockPoint] = anchorCheck; } } else { distanceAnchors.Add(distanceToOriginalLockPoint, anchorCheck); } } // Loop through keys and find the largest distance int maxDistanceFound = -99; foreach (int keyEntry in distanceAnchors.Keys) { if (keyEntry > maxDistanceFound) { maxDistanceFound = keyEntry; } } // If a distance was found that wasn't the initialiser number then assign for unlocking. if (maxDistanceFound != -99) { anchorToUnlock = distanceAnchors[maxDistanceFound]; } } else { // Allow the anchor to be unlocked. anchorToUnlock = requestors[0]; } } // If an anchor point was found to release, then unlock it. // Safety check status will need to be implemented to allow centre point time to move // within restriction zone if restricted area shrinks past it. if (anchorToUnlock != null) { anchorToUnlock.currentAction = BMod_AnchorPoint.AnchorCurrentActionEnum.waitingForBalance; balPosWorkingStatus = balPolStatusEnum.adjustingBalance; // Make sure this is called. //anchorToUnlock.lockedInPosition = false; anchorToUnlock.needsToLift = false; Vector3 nonHeightAnchor = new Vector3(anchorToUnlock.anchorVertex.transform.position.x, 0.0f, anchorToUnlock.anchorVertex.transform.position.z); Vector3 currentPosition = nonHeightAnchor; // Update the unlock position to be slightly above the current position anchorToUnlock.unlockedPosition = new Vector3(currentPosition.x, currentPosition.y + anchorLiftHeight, currentPosition.z); restrictedMovementAreaUpdateRequired = true; } } }