// 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>();
}
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;
}
}
}
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));
}
}
}
// 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;
}
}
