// Update is called once per frame
void FixedUpdate()
{
WPpos.moveTo(new Vector3(targetPoseAndVel.x, targetPoseAndVel.y, 0));
Vector3 targetWP_local = myref.InverseTransformPoint(TargetWP.transform.position);
targetDiss = targetWP_local.magnitude;
LinVelCmd = P_diss * targetDiss;
LinVelCmd = Mathf.Min(LinVelCmd, targetPoseAndVel.z);
targetAzi = Mathf.Atan2(targetWP_local.x, targetWP_local.z);
if (targetAzi > Mathf.PI)
{
targetAzi = targetAzi - 2 * Mathf.PI;
}
else if (targetAzi < -Mathf.PI)
{
targetAzi = targetAzi + 2 * Mathf.PI;
}
AngVelCmd = P_azimuth * targetAzi;
VelController.targeLinearVel = LinVelCmd;
VelController.targetAngularVel = AngVelCmd;
}
void pathConstraction()
{
for (int WP_i = 0; WP_i < PathWPs_PosesAndVels.Count; WP_i++)
{
if (WP_i >= PathWPs.Count)
{
GameObject newWP_Object = Instantiate(PathWP_Mark);
newWP_Object.name = VehicleName + "_PathWP_" + WP_i.ToString();
PathWPs.Add(newWP_Object);
}
terrainAttachment WPpos = PathWPs[WP_i].GetComponent <terrainAttachment>();
WPpos.moveTo(new Vector3(PathWPs_PosesAndVels[WP_i].x, PathWPs_PosesAndVels[WP_i].y, 0));
}
}
void LeaderSetup()
{
foreach (var vehicleXML in file.Descendants("platform_pose"))
{
LeaderPose = new Vector2(float.Parse(vehicleXML.Element("initial_platform_position_on_map_X_axis").Value),
float.Parse(vehicleXML.Element("initial_platform_position_on_map_Y_axis").Value)); // * MapSize;
LeaderAzimuth = float.Parse(vehicleXML.Element("initial_platform_azimut_on_map").Value);
//LeaderVehicle = Instantiate(LeaderVehicleRef);
terrainAttachment LeaderVehiclePos = LeaderVehicle.GetComponent <terrainAttachment>();
LeaderVehiclePos.moveTo(new Vector3(LeaderPose.x, LeaderPose.y, 0.0f));
LeaderVehicle.transform.eulerAngles = new Vector3(0, LeaderAzimuth * Mathf.Rad2Deg, 0);
Debug.Log("Leader position:" + LeaderPose.x.ToString() + "," + LeaderPose.y.ToString());
}
}
// Update is called once per frame
void Update()
{
#if MOVINGSHAHID
TargetMarkPos.moveTo(new Vector3(shahidTargetPoseAndVel.x + targetRadius, shahidTargetPoseAndVel.y + targetRadius, 0));
#endif
Vector3 targetWP_local = myref.InverseTransformPoint(targetWP_Mark.transform.position);
targetDist = targetWP_local.magnitude; //Length of the vector
//Debug.Log("targetDist is:"+targetDist.ToString());
if (targetDist < targetRadius)
{
// Debug.Log("00000 targetDist is zero:"+targetDist.ToString());
targetDist = 0;
}
LinVelCmd = P_dist * targetDist;
LinVelCmd = Mathf.Min(LinVelCmd, shahidTargetPoseAndVel.z);
targetAzi = Mathf.Atan2(targetWP_local.x, targetWP_local.z);
if (targetAzi > Mathf.PI)
{
targetAzi = targetAzi - 2 * Mathf.PI;
}
else if (targetAzi < -Mathf.PI)
{
targetAzi = targetAzi + 2 * Mathf.PI;
}
AngVelCmd = P_azimuth * targetAzi;
#if MOVINGSHAHID
shahid.WalkingCommand = LinVelCmd;
shahid.TurningCommand = AngVelCmd;
Debug.Log("LinVelCmd is:" + LinVelCmd.ToString() + " and AngVelCmd is:" + AngVelCmd.ToString());
#endif
}
void FollowerSetup()
{
float FollowerDiss = 20; //When inserted scenario doesn't exist, the distance between the two vehicle is 20 and the
// lock works better .... it was 30;
Vector2 diffVec = new Vector2(Mathf.Sin(LeaderAzimuth), Mathf.Cos(LeaderAzimuth)) * FollowerDiss;
Vector2 FollowerPose = LeaderPose - diffVec;
float FollowerAzimuth = LeaderAzimuth;
//FollowerVehicle = Instantiate(FollowerVehicleRef);
terrainAttachment FollowerVehiclePos = FollowerVehicle.GetComponent <terrainAttachment>();
FollowerVehiclePos.moveTo(new Vector3(FollowerPose.x, FollowerPose.y, 0.0f));
FollowerVehiclePos.transform.eulerAngles = new Vector3(0, FollowerAzimuth * Mathf.Rad2Deg, 0);
Debug.Log("Follower position:" + FollowerPose.x.ToString() + "," + FollowerPose.y.ToString());
//FollowerVehicle = oshkosh
VehiclePathController FollowerPathController = FollowerVehicle.GetComponent <VehiclePathController>();
FollowerPathController.PathWPs_PosesAndVels.Clear();
VehicleFollowerPathController PathController = FollowerVehicle.GetComponent <VehicleFollowerPathController>();
PathController.leaderVehicle = LeaderVehicle.transform;
}