void FixedUpdate()
{
t = theIntegrator.RK4Step(theIntegrator.X, t, h);
pos = gameObject.transform.position;
//velocity = (pos - previous) / Time.deltaTime;
//previous = pos;
rot = gameObject.transform.rotation.eulerAngles;
pos.x = theIntegrator.X[0];
pos.z = theIntegrator.X[1];
pos.y = theIntegrator.X[2];
rot.x = -theIntegrator.X[6] * 180 / Mathf.PI;
rot.z = -theIntegrator.X[7] * 180 / Mathf.PI;
rot.y = -theIntegrator.X[8] * 180 / Mathf.PI;
/*float[] dat = new float[10];
* dat[0] = pos.x;
* dat[1] = pos.z;
* dat[2] = pos.y;
* dat[3] = velocity.x;
* dat[4] = velocity.z;
* dat[5] = velocity.y;
* dat[6] = -1 * rot.x * Mathf.PI / 180;
* dat[7] = -1 * rot.z * Mathf.PI / 180;
* dat[8] = -1 * rot.y * Mathf.PI / 180;
* dat[9] = t;
* data.Add(dat);*/
theIntegrator.z_d += Input.GetAxis("Throttle");
theIntegrator.phi_d = Input.GetAxis("Roll"); //+ Input.GetAxis("Yaw")
theIntegrator.theta_d = Input.GetAxis("Pitch"); // +Input.GetAxis("Yaw")
theIntegrator.psi_d += Input.GetAxis("Yaw");
f1.setPower(theIntegrator.w1);
f2.setPower(theIntegrator.w2);
f3.setPower(theIntegrator.w3);
f4.setPower(theIntegrator.w4);
Quaternion rotq = Quaternion.Euler(rot.x, rot.y, rot.z);
//gameObject.transform.position = pos;
//gameObject.transform.rotation = rotq;
}
void FixedUpdate()
{
if (StartTrigger)
{
//play runge-kutta method
t = theIntegrator.RK4Step(theIntegrator.X, t, h);
pos = gameObject.transform.position;
velocity = (pos - previous) / Time.deltaTime; //get velocity at Scene ifself
previous = pos;
//verify position, rotation
pos.x = theIntegrator.X[0];
pos.z = theIntegrator.X[1];
pos.y = theIntegrator.X[2];
rot = gameObject.transform.rotation.eulerAngles;
rot.x = -theIntegrator.X[6] * 180 / Mathf.PI;
rot.z = -theIntegrator.X[7] * 180 / Mathf.PI;
rot.y = -theIntegrator.X[8] * 180 / Mathf.PI;
rotq = Quaternion.Euler(rot.x, rot.y, rot.z);
//verify Input
theIntegrator.z_d += Input.GetAxis("Throttle");
theIntegrator.phi_d = Input.GetAxis("Roll");
theIntegrator.theta_d = Input.GetAxis("Pitch");
theIntegrator.psi_d += Input.GetAxis("Yaw");
//verify Rotor Angular Velocity(Power)
f1.setPower(theIntegrator.w1);
f2.setPower(theIntegrator.w2);
f3.setPower(theIntegrator.w3);
f4.setPower(theIntegrator.w4);
}
if (Input.GetAxis("Throttle") > 0.3 && !StartTrigger)
{
StartTrigger = true;
CollisionEnter = false;
}
if (StartTrigger && !CollisionEnter)
{
gameObject.transform.position = pos;
gameObject.transform.rotation = rotq;
ResetTrigger = true;
innertimer += Time.deltaTime;
}
//reset trigger
if ((velocity.magnitude <= 0.01f && CollisionEnter && ResetTrigger) || gameObject.transform.position.y < 18)
{
if (innertimer < 0.5f)
{
this.transform.position = new Vector3(this.transform.position.x, this.transform.position.y + 40, this.transform.position.z);
}
else
{
ResetTrigger = false;
UIButton.posX = gameObject.transform.position.x;
UIButton.posY = gameObject.transform.position.z;
UIButton.posZ = gameObject.transform.position.y;
UIButton.rotZ = gameObject.transform.rotation.eulerAngles.y;
SceneManager.LoadScene("DroneProj");
}
}
}