// Update is called once per frame void FixedUpdate() { ServerMessages.IPSFrameData frameData = DroneModule.DequeueIPSDronFrame(); if (frameData != null) { drone.transform.position = frameData.position; drone.transform.rotation = Quaternion.Euler(frameData.rotation); } //if (droneFlying) { // if (!anchorsActivated) { // anchorIDParent.SetActive(true); // anchorsActivated = true; // } // for (int i = 0; i < anchors.Length; i++) // { // if (GeometryUtility.TestPlanesAABB(GeometryUtility.CalculateFrustumPlanes(Camera.main), anchors[i].GetComponent<Collider>().bounds)) // { // anchorIDPos[i] = Camera.main.WorldToScreenPoint(anchors[i].transform.position); // anchorIDs[i].anchoredPosition = new Vector2(anchorIDPos[i].x - 125, anchorIDPos[i].y + 25); // } // else // { // anchorIDs[i].anchoredPosition = new Vector2(-400, -400); // } // } //} }
// Update is called once per frame void FixedUpdate() { //We get the drone position to reflect it on the screen ServerMessages.IPSFrameData frameData = DroneModule.DequeueIPSDronFrame(); if (frameData != null) { drone.transform.position = frameData.position; drone.transform.rotation = Quaternion.Euler(frameData.rotation); //Camera.main.transform.position = new Vector3(drone.transform.position.x, drone.transform.position.y + 10, drone.transform.position.z) ; } //We get the anchors to paint them ServerMessages.IPSFrameAnchorData anchorData = PozyxModule.DequeueIPSAnchorFrame(); if (anchorData != null) { anchorParent.transform.Find("Anchor" + anchorID).position = anchorData.position; anchorID++; if (anchorID == 9) { UnityEngine.Debug.Log("All anchors positioned"); } } //Here we should do a takeoff switch (states) { case FlyingStates.IDLE: break; case FlyingStates.STARTFLYING: time += Time.deltaTime; if (time >= 2.0f) { clientUnity.client.SendCommand((byte)Modules.PLAN_EXECUTOR_MODULE, (byte)PlanExecutorCommandType.PLAN_EXEC_TAKEOFF); states = FlyingStates.FLYING; } break; case FlyingStates.FLYING: break; default: break; } //If the drone reached its destination, we activate the button that returns to general if (flightEnded) { flightEnded = false; droneHasFinishedPanel.gameObject.SetActive(true); warningRecStartPanel.gameObject.SetActive(false); recordingPanel.gameObject.SetActive(false); } /* * if (indexWaypoints >= waypoints.Count || TotalindexMiddlePoints >= path.middlePointsTop.Count) * { * active = false; * * } * if (active && time < 1) * { * drone.transform.position = Vector3.Lerp(actualWaypoint, nextWaypoint, time); * //camToTravel.transform.position = nextWaypoint * Time.deltaTime * 0.001f; * if (waypoints[indexWaypoints].GetComponent<PathPoint>().BlockDirection) * { * drone.transform.rotation = Quaternion.Euler(waypoints[indexWaypoints].GetComponent<PathPoint>().GimbalRotation.z, waypoints[indexWaypoints].GetComponent<PathPoint>().GimbalRotation.y - 90, waypoints[indexWaypoints].GetComponent<PathPoint>().GimbalRotation.x); * //blockedDIrection = true; * } * else if (!waypoints[indexWaypoints].GetComponent<PathPoint>().BlockDirection) * { * drone.transform.LookAt(waypoints[indexWaypoints].GetComponent<PathPoint>().Poi); * } * * time += Time.deltaTime; * } * if (active && time >= 1) * { * actualWaypoint = nextWaypoint; * UnityEngine.Debug.Log(waypoints[indexWaypoints].GetComponent<PathPoint>().Segments); * if (waypoints[indexWaypoints].GetComponent<PathPoint>().Segments <= 3) * { * if (waypoints.Count - 1 > indexWaypoints) * { * indexWaypoints++; * TotalindexMiddlePoints++; * nextWaypoint = waypoints[indexWaypoints].transform.position; * indexMiddlePoints = 0; * } * //blockedDIrection = false; * * } * else * { * if (waypoints[indexWaypoints].GetComponent<PathPoint>().Segments == indexMiddlePoints + 1) * { * indexWaypoints++; * TotalindexMiddlePoints++; * if (TotalindexMiddlePoints < path.middlePointsTop.Count) * nextWaypoint = path.middlePointsTop[TotalindexMiddlePoints]; * indexMiddlePoints = 0; * //blockedDIrection = false; * * } * else if (path.middlePointsTop.Count > TotalindexMiddlePoints) * { * indexMiddlePoints++; * TotalindexMiddlePoints++; * if (TotalindexMiddlePoints < path.middlePointsTop.Count) * nextWaypoint = path.middlePointsTop[TotalindexMiddlePoints]; * } * } * * * time = 0; * } */ }
// Update is called once per frame void FixedUpdate() { switch (state) { case CalibrationState.IDLE: //The spinning wheel(throbber) rotates if (autocalibratingThrobber.activeInHierarchy) { autocalibratingThrobber.transform.Rotate(0, 0, -200 * Time.deltaTime); } break; case CalibrationState.DISCOVERING: searchingLabel.transform.GetChild(0).Rotate(0, 0, -200 * Time.deltaTime); break; case CalibrationState.RECEIVING_DISCOVER_ANCHORS: List <int> anchorsIDList = PozyxModule.GetAnchorList(); { //UnityEngine.Debug.Log("ReadingAnchorList"); dicoveredAnchors.text = ""; for (int i = 0; i < anchorsIDList.Count; i++) { //Writing the anchor list received inside the TextBoxes that can be clicked, with the HorizontalGroup name //UnityEngine.Debug.Log("Reading Anchor List Element: " + i); //anchorParent.transform.Find("Cube" + anchorID).position = anchorData.position; //anchor1Pos = cam.WorldToScreenPoint(anchorData.position); //transform.Find("Cube" + anchorID + "ID").GetComponent<RectTransform>().anchoredPosition = new Vector2(anchor1Pos.x - 40, anchor1Pos.y - Screen.height * 0.15f); dicoveredAnchors.text += "Anchor: " + i + " ID: " + "0x" + anchorsIDList[i].ToString("x") + "\n"; anchorList.transform.Find("HorizontalGroup" + (i + 1)).GetChild(0).GetChild(0).GetComponent <Text>().text = "0x" + anchorsIDList[i].ToString("x"); anchorList.transform.Find("HorizontalGroup" + (i + 1)).GetChild(0).name = anchorsIDList[i].ToString(); } //UnityEngine.Debug.Log("FinishedReadingAnchorList"); //Activate the button that lets the user give the correct position to the anchors configAnchorsButton.GetComponent <Button>().interactable = true; configAnchorsButton.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1, 1, 1, 1); //UnityEngine.Debug.Log("All anchors positioned"); searchingLabel.SetActive(false); state = CalibrationState.RECEIVED_DISCOVER_ANCHORS; } break; case CalibrationState.RECEIVED_DISCOVER_ANCHORS: //Having done previously that, is this state necessary? if (PozyxModule.anchorsReceived) { PozyxModule.anchorsReceived = false; state = CalibrationState.DISCOVERED; } break; case CalibrationState.DISCOVERED: anchorReadCount = 0; for (int i = 0; i < 8; i++) { ServerMessages.IPSFrameAnchorData anchorData = CalibrationSettings.GetAnchorData(i); //anchorList.transform.Find("HorizontalGroup" + (anchorData.order + 1)).GetChild(0).GetChild(0).GetComponent<Text>().text = anchorData.id.ToString(); //anchorList.transform.Find("HorizontalGroup" + (anchorData.order + 1)).GetChild(0).name = anchorData.id.ToString(); //Writing the corresponding id of an anchor to a cube anchorCubeParent.transform.Find("Anchor" + (anchorData.order + 1) + "Cube").GetChild(0).GetComponent <TextMesh>().text = "0x" + anchorData.id.ToString("x"); anchorCubeParent.transform.Find("Anchor" + (anchorData.order + 1) + "Cube").GetChild(1).localPosition = anchorData.position; for (int j = 1; j < 9; j++) { //UnityEngine.Debug.Log(anchorData.order); //Writing the received position of the anchors if (anchorList.transform.Find("HorizontalGroup" + j).GetChild(0).name == anchorData.id.ToString()) { anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().text = anchorData.position.x.ToString(); anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().text = anchorData.position.y.ToString(); anchorList.transform.Find("HorizontalGroup" + j).GetChild(3).GetComponent <InputField>().text = anchorData.position.z.ToString(); anchorList.transform.Find("HorizontalGroup" + j).GetChild(0).GetComponent <Image>().color = new Vector4(0, 255, 0, 150); anchorReadCount++; //Writing the received position of the anchor list when autocalibration is pressed if (autoCalib && (anchorData.order == 1 || anchorData.order == 5)) { anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().text = ""; anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().placeholder.GetComponent <Text>().text = "AUTO"; anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().interactable = false; anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().text = "0"; } else if (autoCalib && (anchorData.order == 3 || anchorData.order == 7)) { anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().text = "0"; anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().text = ""; anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().placeholder.GetComponent <Text>().text = "AUTO"; anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().interactable = false; } else if (autoCalib && (anchorData.order == 2 || anchorData.order == 6)) { anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().text = ""; anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().placeholder.GetComponent <Text>().text = "AUTO"; anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().interactable = false; anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().text = ""; anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().placeholder.GetComponent <Text>().text = "AUTO"; anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().interactable = false; } else if (autoCalib) { anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().text = "0"; anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().text = "0"; } } } } //UnityEngine.Debug.Log("Anclas coincidentes: " + anchorReadCount); //Activates button to confirm configuration if (anchorReadCount == 8) { UnityEngine.Debug.Log("PozyxModuleAnchorsReady"); confirmManual.interactable = true; confirmManual.transform.GetChild(0).GetComponent <Text>().color = new Vector4(255, 255, 255, 255); } state = CalibrationState.IDLE; //anchorParent.transform.Find("Cube" + anchorID).position = anchorData.position; //anchor1Pos = cam.WorldToScreenPoint(anchorData.position); //transform.Find("Cube" + anchorID + "ID").GetComponent<RectTransform>().anchoredPosition = new Vector2(anchor1Pos.x - 40, anchor1Pos.y - Screen.height * 0.15f); //dicoveredAnchors.text += "Tag: " + anchorID + " ID: " + anchorData.id + "\n"; //anchorList.transform.Find("Anchor" + anchorID).GetChild(0).GetComponent<Text>().text = "Anchor ID: " + anchorData.id; //anchorList.transform.Find("Anchor" + anchorID).name = anchorData.id.ToString(); //configAnchorsButton.GetComponent<Button>().interactable = true; //configAnchorsButton.transform.GetChild(0).GetComponent<Text>().color = new Vector4(1, 1, 1, 1); //UnityEngine.Debug.Log("All anchor data received and ready to be used"); //searchingLabel.SetActive(false); //ipsCommandSent = false; break; case CalibrationState.MANUAL_ACCEPTED: //Sending the configuration and wait for positioning clientUnity.client.SendCommand((byte)Modules.POSITIONING_MODULE, (byte)PositioningCommandType.IPS_UPDATE_SETTINGS); state = CalibrationState.TRY_START_POSITIONING; break; case CalibrationState.AUTOCALIBRATION_RECEIVED: autocalibratingThrobber.transform.Rotate(0, 0, -200 * Time.deltaTime); anchorReadCount = 0; for (int i = 0; i < 8; i++) { ServerMessages.IPSFrameAnchorData anchorData = CalibrationSettings.GetAnchorData(i); //anchorList.transform.Find("HorizontalGroup" + (anchorData.order + 1)).GetChild(0).GetChild(0).GetComponent<Text>().text = anchorData.id.ToString(); //anchorList.transform.Find("HorizontalGroup" + (anchorData.order + 1)).GetChild(0).name = anchorData.id.ToString(); anchorCubeParent.transform.Find("Anchor" + (anchorData.order + 1) + "Cube").GetChild(0).GetComponent <TextMesh>().text = "0x" + anchorData.id.ToString("x"); anchorCubeParent.transform.Find("Anchor" + (anchorData.order + 1) + "Cube").GetChild(1).localPosition = anchorData.position; //Receiving the anchors position from autocalibration for (int j = 1; j < 9; j++) { if (anchorList.transform.Find("HorizontalGroup" + j).GetChild(0).name == anchorData.id.ToString()) { anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().text = anchorData.position.x.ToString(); anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().text = anchorData.position.y.ToString(); anchorList.transform.Find("HorizontalGroup" + j).GetChild(3).GetComponent <InputField>().text = anchorData.position.z.ToString(); anchorList.transform.Find("HorizontalGroup" + j).GetChild(0).GetComponent <Button>().interactable = false; anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().interactable = false; anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().interactable = false; anchorList.transform.Find("HorizontalGroup" + j).GetChild(3).GetComponent <InputField>().interactable = false; anchorList.transform.Find("HorizontalGroup" + j).GetChild(0).GetComponent <Image>().color = new Vector4(0, 255, 0, 150); anchorReadCount++; //Painting orange the autocalibrated anchors if (anchorData.order == 1 || anchorData.order == 2 || anchorData.order == 5 || anchorData.order == 6) { ColorBlock cb = anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().colors; cb.disabledColor = new Vector4(1.0f, 0.6f, 0.0f, 0.4f); cb.normalColor = new Vector4(1.0f, 0.8f, 0.4f, 1.0f); cb.highlightedColor = new Vector4(1.0f, 0.9f, 0.7f, 1.0f); anchorList.transform.Find("HorizontalGroup" + j).GetChild(1).GetComponent <InputField>().colors = cb; } if (anchorData.order == 2 || anchorData.order == 3 || anchorData.order == 6 || anchorData.order == 7) { ColorBlock cb = anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().colors; cb.disabledColor = new Vector4(1.0f, 0.6f, 0.0f, 0.4f); cb.normalColor = new Vector4(1.0f, 0.8f, 0.4f, 1.0f); cb.highlightedColor = new Vector4(1.0f, 0.9f, 0.7f, 1.0f); anchorList.transform.Find("HorizontalGroup" + j).GetChild(2).GetComponent <InputField>().colors = cb; } } } } if (anchorReadCount == 8) { //Habilitating the three buttons //UnityEngine.Debug.Log("AnchorsAutocalibratedReady"); // ## Se puede sacar fuera del if todo confirmManual.gameObject.SetActive(false); editConfig.gameObject.SetActive(true); redoAutocalibration.gameObject.SetActive(true); confirmAuto.gameObject.SetActive(true); editConfig.interactable = true; editConfig.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1.0f, 1.0f, 1.0f, 1.0f); redoAutocalibration.interactable = true; redoAutocalibration.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1.0f, 1.0f, 1.0f, 1.0f); confirmAuto.interactable = true; confirmAuto.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1.0f, 1.0f, 1.0f, 1.0f); //acceptConfig.interactable = true; //acceptConfig.transform.GetChild(0).GetComponent<Text>().color = new Vector4(255, 255, 255, 255); } state = CalibrationState.AUTOCALIBRATION_FINNISHED; break; case CalibrationState.AUTOCALIBRATION_ACCEPTED: autocalibratingThrobber.transform.Rotate(0, 0, -200 * Time.deltaTime); // Used to check the current state break; case CalibrationState.AUTOCALIBRATION_FINNISHED: // Used to check the current state autocalibratingThrobber.transform.parent.gameObject.SetActive(false); break; case CalibrationState.CALIBRATION_PREVIEW: //Nobody uses this state ?? ServerMessages.IPSFrameData frameData = DroneModule.DequeueIPSDronFrame(); if (frameData != null) { //UnityEngine.Debug.Log("Position: " + frameData.position); //UnityEngine.Debug.Log("Rotation: " + frameData.rotation); drone.transform.position = frameData.position; drone.transform.rotation = Quaternion.Euler(frameData.rotation); } anchor1Pos = cam.WorldToScreenPoint(anchor1GO.transform.position); anchor2Pos = cam.WorldToScreenPoint(anchor2GO.transform.position); anchor3Pos = cam.WorldToScreenPoint(anchor3GO.transform.position); anchor4Pos = cam.WorldToScreenPoint(anchor4GO.transform.position); anchor5Pos = cam.WorldToScreenPoint(anchor5GO.transform.position); anchor6Pos = cam.WorldToScreenPoint(anchor6GO.transform.position); anchor7Pos = cam.WorldToScreenPoint(anchor7GO.transform.position); anchor8Pos = cam.WorldToScreenPoint(anchor8GO.transform.position); // ## TODO: Incluir en el if de arriba buscando cada anchorID según el ID actual y asociar su posición anchor1ID.anchoredPosition = new Vector2(anchor1Pos.x - 40, anchor1Pos.y - Screen.height * 0.15f); anchor2ID.anchoredPosition = new Vector2(anchor2Pos.x - 40, anchor2Pos.y - Screen.height * 0.15f); anchor3ID.anchoredPosition = new Vector2(anchor3Pos.x - 40, anchor3Pos.y - Screen.height * 0.15f); anchor4ID.anchoredPosition = new Vector2(anchor4Pos.x - 40, anchor4Pos.y - Screen.height * 0.15f); anchor5ID.anchoredPosition = new Vector2(anchor5Pos.x - 40, anchor5Pos.y - Screen.height * 0.15f); anchor6ID.anchoredPosition = new Vector2(anchor6Pos.x - 40, anchor6Pos.y - Screen.height * 0.15f); anchor7ID.anchoredPosition = new Vector2(anchor7Pos.x - 40, anchor7Pos.y - Screen.height * 0.15f); anchor8ID.anchoredPosition = new Vector2(anchor8Pos.x - 40, anchor8Pos.y - Screen.height * 0.15f); break; case CalibrationState.TRY_START_POSITIONING: //Check for positioning if (PozyxModule.positiningIsValid) { firstPositionTry = true; state = CalibrationState.POSITIONING_CONFIRMED; } else { if (firstPositionTry) { autocalibratingThrobber.transform.parent.GetComponent <Text>().text = "Waiting for positioning"; autocalibratingThrobber.transform.parent.gameObject.SetActive(true); firstPositionTry = false; } autocalibratingThrobber.transform.Rotate(0, 0, -200 * Time.deltaTime); } break; case CalibrationState.POSITIONING_CONFIRMED: //Change scene, positioning received state = CalibrationState.IDLE; autocalibratingThrobber.transform.parent.gameObject.SetActive(false); //UnityEngine.Debug.Log(GeneralSceneManager.appState); //You can access the scene from both buttons, as calibrating is always necessary, that is way there is a button to use last calibration and skip the process if (GeneralSceneManager.appState == GeneralSceneManager.ApplicationState.Mapping) { SceneManager.LoadScene("Mapping"); } else if (GeneralSceneManager.appState == GeneralSceneManager.ApplicationState.Recording) { SceneManager.LoadScene("PlanSelection"); } break; case CalibrationState.LAST_REQ_FAILED: UnityEngine.Debug.Log("LastReqFailed"); if (discoverPanel.activeInHierarchy) { // Something failed discovering anchors, retry UnityEngine.Debug.Log("DiscoverFailed"); dicoveredAnchors.text = PozyxModule.errorMsg; state = CalibrationState.DISCOVERING; } else { // Something failed with anchors configuration, let the user change the config and try again UnityEngine.Debug.Log("ConfigFailed"); state = CalibrationState.IDLE; autocalibratingThrobber.transform.parent.gameObject.SetActive(false); if (confirmManual.gameObject.activeInHierarchy) { confirmManual.interactable = true; confirmManual.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1.0f, 1.0f, 1.0f, 1.0f); } else if (confirmAuto.gameObject.activeInHierarchy) { confirmAuto.interactable = true; confirmAuto.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1.0f, 1.0f, 1.0f, 1.0f); editConfig.interactable = true; editConfig.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1.0f, 1.0f, 1.0f, 1.0f); redoAutocalibration.interactable = true; redoAutocalibration.transform.GetChild(0).GetComponent <Text>().color = new Vector4(1.0f, 1.0f, 1.0f, 1.0f); } } //discoverButton.SetActive(true); //configAnchorsButton.SetActive(false); //startingLabel.SetActive(true); //searchingLabel.SetActive(false); break; } }