void Update()
{
currPosition = transform.position;
currRotation = transform.rotation;
// Stimulate for starting check and trainning time check
var theClient = GameObject.Find("PressController").GetComponent <ForPress>();
signal = theClient.signal;
//--------------------------------------------------------------------------
if (signal[1] < BeginningCommmand || signal[1] > FinalCommmand)
{ // If exception, than default case. And checking the default command(protocol == 0).
command = 0;
}
else
{
for (int i = BeginningCommmand; i <= FinalCommmand; i++)
{
if (signal[1] >= i && signal[1] < (i + 1)) // If it was 1, then (1 <= x < 2).
{
command = i;
break;
}
}
}
//--------------------------------------------------------------------------
if (command == 0)
{
Speed2p = 2;
}
else if (command == 1)
{
Speed2p = 2.5f;
}
else if (command == 2)
{
Speed2p = 3.0f;
}
else if (command == 3)
{
Speed2p = 3.3f;
}
else if (command == 4)
{
Speed2p = 3.7f;
}
else if (command == 5)
{
Speed2p = 4.0f;
}
else if (command == 6)
{
Speed2p = 4.2f;
}
else if (command == 7)
{
Speed2p = 4.3f;
}
else if (command == 8)
{
Speed2p = 4.5f;
}
else if (command == 9)
{
Speed2p = 4.8f;
}
else
{
Speed2p = 5.0f;
}
if (waypointIndex < waypoints.Length)
{
// moving forward
//transform.position = Vector3.MoveTowards(currPosition, waypoints[waypointIndex], speed);
// calculating rotation angle
Vector3 standard = new Vector3(currPosition.x, currPosition.y, currPosition.z + 20);
/*
* double angle = Meas_Angle( standard, currPosition, waypoints[waypointIndex]);
* if ( waypoints[waypointIndex].x - currPosition.x < 0)
* angle = 360 - angle;
*/
Quaternion Right = Quaternion.identity;
//Right.eulerAngles = new Vector3(0, (float)angle, 0);
Quaternion Current = Quaternion.identity;
/*
* if (transform.eulerAngles.y != 0f)
* {
* angle2 = transform.eulerAngles.y;
* }*/
angle2 = transform.eulerAngles.y;
Current.eulerAngles = new Vector3(0, angle2, 0);
float kh_angle = kh_get_angle(currPosition, standard, waypoints[waypointIndex]);
kh_angle = Mathf.Round(kh_angle * Mathf.Pow(10, 4)) * 0.0001f;
if (waypoints[waypointIndex].x - currPosition.x < 0)
{
kh_angle = 360.0f - kh_angle; // reverse?
}
angle2 = Mathf.Round(angle2 * Mathf.Pow(10, 4)) * 0.0001f;
Right.eulerAngles = new Vector3(0, kh_angle, 0);
//transform.rotation = Quaternion.Slerp(Current, Right, Time.deltaTime * 6.0f);
transform.Translate(new Vector3(0, 0, (int)Speed2p)); // move forward (only z-axis)
var dir_right = Quaternion.Euler(0, 55.0f, 0) * transform.forward;
var dir_left = Quaternion.Euler(0, -55.0f, 0) * transform.forward;
Ray ray = new Ray(this.transform.position, this.transform.forward);
/*
* Ray ray_right = new Ray(this.transform.position, (this.transform.right + this.transform.forward).normalized);
* Ray ray_left = new Ray(this.transform.position, (this.transform.forward - this.transform.right).normalized);
*/
Ray ray_right = new Ray(this.transform.position, dir_right);
Ray ray_left = new Ray(this.transform.position, dir_left);
//transform.ri
RaycastHit hit_forward; // forward
RaycastHit hit_right;
RaycastHit hit_left;
float diff_distance = 0;
Physics.Raycast(ray, out hit_forward, 1000);
Physics.Raycast(ray_right, out hit_right, 1000);
Physics.Raycast(ray_left, out hit_left, 1000);
diff_distance = hit_left.distance - hit_right.distance;
// ------------------ Wheel direction control using Unity Raycast features
// Raycast shoots layser to the assigned direction and measrues distance
// Raycast information: forward, forward + right, and forward + left
// Case:
// - when left distance is shorter than right distance
// : turn right
// - when right distance is shorter than left distance
// : turn left
// now turning step is optimized in case of speed=3 (normalization is done in terms of 3)
if (diff_distance > 15.0f) // control sensitivity level
{
transform.Rotate(new Vector3(0, -1.2f * (int)Speed2p / 3.0f, 0)); // turn left with fine steps
}
else if (diff_distance < -15.0f) // control sensitivity level
{
transform.Rotate(new Vector3(0, 1.2f * (int)Speed2p / 3.0f, 0)); // turn right with fine steps
}
/*
* Debug.Log("forward:"+ hit_forward.distance+" left:" + hit_left.distance + " right:" + hit_right.distance + " diff:" +
* diff_distance+ " speed: "+speed);
*/
if (Vector3.Distance(waypoints[waypointIndex], currPosition) <= 0.01f)
{
waypointIndex++;
if (waypointIndex == 21)
{
waypointIndex = 0;
}
}
Marker2.transform.position = new Vector3(this.transform.position.x, -1290, this.transform.position.z);
SpeedobarConverter2.ShowSpeed((int)Speed2p, 0, 10);
}
}
// Use this for initialization
void Start()
{
Speedobar = this;
}