Inheritance: MonoBehaviour
 // Use this for initialization
 void Start()
 {
     pos     = new Vector3();
     dataObj = GetComponent <VisionData>();
     offset  = new Vector3(0, 0, 0);
     rb      = GameObject.FindGameObjectWithTag("kart").GetComponent <Rigidbody>();
 }
 // Use this for initialization
 void Start()
 {
     pos = new Vector3 ();
     dataObj = GetComponent<VisionData>();
     offset = new Vector3(0,0,0);
     rb = GameObject.FindGameObjectWithTag("kart").GetComponent<Rigidbody>();
 }
Exemple #3
0
 void Update()
 {
     if (!done)
     {
         VisionData dataObj = rec.getDataObj(0);
         if (dataObj != null)
         {
             Vector3 v = new Vector3(dataObj.positionX, 0, dataObj.positionY) * MoveFromVisionData1.SCALE;
             transform.position = v - start.localPosition + new Vector3(0, 0.02f, 0);
             done = true;
         }
     }
 }
Exemple #4
0
    public VisionDataHandling()
    {
        // Get the Unity world dimensions.
        bot = GameObject.Find("Bot");
        var groundObject = GameObject.Find("Ground");

        xMinWorld = groundObject.GetComponent <Renderer> ().bounds.min.x;
        xMaxWorld = groundObject.GetComponent <Renderer> ().bounds.max.x;
        zMinWorld = groundObject.GetComponent <Renderer> ().bounds.min.z;
        zMaxWorld = groundObject.GetComponent <Renderer> ().bounds.max.z;

        visionData = bot.GetComponent <BotController> ().visionData;
        // Crop the camera image to match the beamer projection.
        var marginCameraX = visionData.videoSize [0] * VisionConstants.CameraMargin;
        var marginCameraZ = visionData.videoSize [1] * VisionConstants.CameraMargin;

        xMinCamera = marginCameraX;
        xMaxCamera = visionData.videoSize [0] - marginCameraX;
        zMinCamera = marginCameraZ;
        zMaxCamera = visionData.videoSize [1] - marginCameraZ;
    }
    private void GetHighestConfidence(VisionData data)
    {
        float        maxConfidence = 0;
        VisionObject obj           = null;

        if (data != null)
        {
            foreach (var vobj in data.objects)
            {
                if (vobj.confidence > maxConfidence)
                {
                    maxConfidence = vobj.confidence;
                    obj           = vobj;
                }
            }
            if (obj != null)
            {
                int    midX      = (obj.rectangle.x + obj.rectangle.w) / 2;
                int    midY      = (obj.rectangle.y + obj.rectangle.h) / 2;
                String objString = obj.obj;
                StartCoroutine(audioManager.playWord(midX, midY, objString));
            }
        }
    }
Exemple #6
0
    // This function is called every fixed framerate frame, if the MonoBehaviour is enabled.
    // FixedUpdate should be used instead of Update when dealing with Rigidbody.
    // For example when adding a force to a rigidbody, you have to apply the force every fixed frame inside FixedUpdate instead of every frame inside Update.
    void FixedUpdate()
    {
        // We connect / disconnect with the EV3 in this thread (and not in the GUI thread) because the EV3 SendMessage and ReceiveMessage also happen in this thread.
        if (guiConnectEV3)
        {
            if (myEV3.Connect("1234", ipAddress) == true)
            {
                Debug.Log("EV3 connection succeeded");
                // Set calibrated to false right after a switch from simulation to physical or vice versa to prevent jumping of position.
                calibrated = false;
            }
            else
            {
                Debug.Log("EV3 connection failed");
            }
            guiConnectEV3 = false;
        }
        else if (guiDisconnectEV3)
        {
            myEV3.Disconnect();
            // Set calibrated to false right after a switch from simulation to physical or vice versa to prevent jumping of position.
            calibrated = false;
            // Indicate disconnection request is handled.
            guiDisconnectEV3 = false;
        }
        if (guiConnectVision)
        {
            myVision.Connect();
            // Indicate connection request is handled.
            guiConnectVision = false;
        }
        else if (guiDisconnectVision)
        {
            myVision.Disconnect();
            // Indicate disconnection request is handled.
            guiDisconnectVision = false;
        }

        if (guiReset)
        {
            myEV3.SendMessage("Reset", "0");
            angleMoved            = 0.0f;
            distanceMoved         = 0.0f;
            angleMovedPrevious    = 0.0f;
            distanceMovedPrevious = 0.0f;
            rb.transform.position = startPosition;
            rb.rotation           = startRotation;
            guiReset = false;
            // Wait for bot to reset and flush the current message which contains pre-reset values.
            Thread.Sleep(500);
            myEV3.ReceiveMessage("EV3_OUTBOX0");
            gotoTarget                 = false;
            shootTheBall               = false;
            isBehindTheBall            = false;
            lineRenderer.positionCount = 0;             // Erase the current path
        }


        // Receive vision data.
        // The ball position is updated in the scene.
        if (myVision.isConnected)
        {
            string msg = myVision.ReceiveMessage();
            visionData = JsonConvert.DeserializeObject <VisionData> (msg);
            // Create myVisionDataHandling after visionData is valid.
            if (myVisionDataHandling == null)
            {
                myVisionDataHandling = new VisionDataHandling();
            }
            targetObject.transform.position = myVisionDataHandling.CameraToWorldCoordinates(new Vector3(visionData.ball [1], ballRadius, visionData.ball [2]));
        }
        else
        {
            visionData = null;
        }

        ms = DateTime.Now.Ticks / TimeSpan.TicksPerMillisecond;
        if (ms - msPrevious > BotConstants.TimeTickMs)
        {
            // Below code is executed once per TimeTickMs. This to limit communication to a physical bot.

            // Read input keys. Because we arrive here once in so many calls of FixedUpdate we have to use key events which last long enough.
            float moveHorizontal         = Input.GetAxis("Horizontal");
            float moveVertical           = Input.GetAxis("Vertical");
            bool  fPressed               = Input.GetKey(KeyCode.F);
            bool  bPressed               = Input.GetKey(KeyCode.B);
            bool  tPressed               = Input.GetKey(KeyCode.T);
            bool  gPressed               = Input.GetKey(KeyCode.G);
            bool  leftMouseButtonClicked = Input.GetMouseButton(1);
            // Get mouse position and convert to World coordinates.
            Vector3 mPosition = Input.mousePosition;
            mPosition.z = Camera.main.gameObject.transform.position.y;
            mPosition   = Camera.main.ScreenToWorldPoint(mPosition);

            if (leftMouseButtonClicked)
            {
                // Keep the y position.
                mPosition.y = ballRadius;
                targetObject.transform.position = mPosition;
            }

            Input.ResetInputAxes();                      // To prevent double input.
            if (moveVertical > 0)                        // Forward
            {
                myEV3.SendMessage("Move 0 10 30", "0");  // Move angle (-180 .. 180) distance (cm) power (0..100)
            }
            else if (moveVertical < 0)                   // Backward
            {
                myEV3.SendMessage("Move 0 -10 30", "0"); // Move angle (-180 .. 180) distance (cm) power (0..100)
            }
            else if (moveHorizontal < 0)                 // Left
            {
                myEV3.SendMessage("Move -15 0 30", "0"); // Move angle (-180 .. 180) distance (cm) power (0..100)
            }
            else if (moveHorizontal > 0)                 // Right
            {
                myEV3.SendMessage("Move 15 0 30", "0");  // Move angle (-180 .. 180) distance (cm) power (0..100)
            }
            else if (fPressed)
            {
                myEV3.SendMessage("Move 0 5000 30", "0");                       // Move angle (-180 .. 180) distance (cm) power (0..100)
            }
            else if (bPressed)
            {
                myEV3.SendMessage("Move 0 -5000 30", "0");                      // Move angle (-180 .. 180) distance (cm) power (0..100)
            }
            else if (tPressed)
            {
                // Execute task: Move to target object.
                gotoTarget = true;
            }
            else if (gPressed)
            {
                // Execute task: Shoot the ball.
                shootTheBall    = true;
                isBehindTheBall = false;
            }

            if (gotoTarget)
            {
                gotoTarget = !GotoWayPoint(targetObject.transform.position);
            }
            else if (shootTheBall)
            {
                if (!isBehindTheBall)
                {
                    ballPosition = targetObject.transform.position;
                    // Only continue if the ball is not behind the goal.
                    if (Math.Abs(ballPosition.x) < Math.Abs(goalPosition.x))
                    {
                        Vector3 fromBehindGoalToBall2D = ballPosition - behindGoalPosition;
                        // Make 2D.
                        fromBehindGoalToBall2D.y   = 0;
                        fromBehindGoalToBall2DNorm = fromBehindGoalToBall2D.normalized;
                        isBehindTheBall            = GotoWayPoint(ballPosition + fromBehindGoalToBall2DNorm * (botLength / 2 + ballRadius + BotConstants.BehindTheBallDistance));
                    }
                }
                else
                {
                    // Now go to the ball to shoot. The bot will be BehindTheBallDistance behind the ball.
                    // Make sure BehindTheBallDistance - ShootingDistance is less or equal than RecalculatePathDistance so that the shot can be taken in one move,
                    // otherwise the shot will be interrupted by a recalculation.
                    isBehindTheBall = !GotoWayPoint(ballPosition + fromBehindGoalToBall2DNorm * (botLength / 2 + ballRadius + BotConstants.ShootingDistance));
                    if (!isBehindTheBall)
                    {
                        // After shooting drive backwards a bit to make sure:
                        // - There is no contact with the ball anymore and the bot does not obscure the ball for the camera.
                        // - The bot stays behind the ball even when the ball moves a bit backwards (e.g. due to a tilted floor).
                        myEV3.SendMessage("Move 0 -20 30", "0");                                // Move angle (-180 .. 180) distance (cm) power (0..100)
                        msPreviousTask = ms;
                    }
                }
            }

            // Receive data from bot and update the bot position in the scene.
            // If vision is connected, the bot position is updated using the vision data.
            string strMessage = myEV3.ReceiveMessage("EV3_OUTBOX0");
            // Check if the message is valid. The first message received after connecting with the EV3 is "Accept:EV340"
            // indicating that the connection has been established. This is not a valid message.
            if (strMessage != "")
            {
                string[] data = strMessage.Split(' ');
                if (data.Length == 4)
                {
                    int taskReadyDirect;
                    if (int.TryParse(data [0], out taskReadyDirect) && float.TryParse(data [1], out angleMoved) && float.TryParse(data [2], out distanceMoved) && float.TryParse(data [3], out distanceToObject))
                    {
                        // If a new task just has been sent to the bot, the taskReady will still be on '1'.
                        // It takes about 3 time ticks of 100 ms to receive taskReady = 0 back from the bot.
                        // Therefore we force taskReady to 0 for 5 time ticks after the task has been sent.
                        if (ms - msPreviousTask < 5 * BotConstants.TimeTickMs)
                        {
                            taskReadyDirect = 0;
                        }

                        // Delay taskReady going from 0 (not ready) to 1 (ready).
                        // This is needed when the taskReady message of a physical bot arrives earlier than the position update of the visionData.
                        if (taskReadyDirect == 1)
                        {
                            if (taskReadyPrevious == 0)
                            {
                                taskReady = 0;
                                msStartTaskReadyExtension = ms;
                            }
                            else if (ms - msStartTaskReadyExtension > BotConstants.TaskReadyDelay)
                            {
                                taskReady = 1;
                            }
                        }
                        else
                        {
                            taskReady = 0;
                        }
                        taskReadyPrevious = taskReadyDirect;

                        float distanceMovedDelta = distanceMoved - distanceMovedPrevious;
                        float angleMovedDelta    = angleMoved - angleMovedPrevious;
                        distanceMovedPrevious = distanceMoved;
                        angleMovedPrevious    = angleMoved;
                        if (calibrated)
                        {
                            if (visionData == null)
                            {
                                Quaternion rot = Quaternion.Euler(0, angleMovedDelta, 0);
                                rb.MoveRotation(rb.rotation * rot);
                                rb.MovePosition(rb.transform.position + distanceMovedDelta * rb.transform.forward);
                            }
                            else if (visionData.bot1 [0] <= 360)
                            {
                                rb.transform.position = myVisionDataHandling.CameraToWorldCoordinates(new Vector3(visionData.bot1 [1], botHeight, visionData.bot1 [2]));;
                                rb.rotation           = Quaternion.Euler(0, visionData.bot1 [0], 0);
                            }
                        }
                        calibrated = true;
                        if (!myEV3.simOnly && false)
                        {
                            // Draw a wall in front of the bot when an object is detected.
                            // Do not do this in simulation mode as this wall might collide with the bot at distance 0.
                            var          wallObject = GameObject.Find("Wall");
                            MeshRenderer renderWall = wallObject.GetComponentInChildren <MeshRenderer> ();
                            Vector3      vec        = new Vector3();
                            vec.x = rb.transform.position.x;
                            vec.y = wallObject.transform.position.y;                             // Original height.
                            vec.z = rb.transform.position.z;
                            wallObject.transform.position = vec;
                            wallObject.transform.rotation = rb.transform.rotation;
                            // Place the wall in front of the bot at distanceToObject units.
                            wallObject.transform.Translate(0, 0, distanceToObject + 10);                              // +10 to compensate for bot length.
                            if (distanceToObject >= 0 && distanceToObject < 30)
                            {
                                renderWall.enabled = true;
                            }
                            else
                            {
                                renderWall.enabled = false;
                            }
                        }
                    }
                }
            }
            msPrevious = ms;
        }
    }