public void pasoPIDHybridTest()
{
GameObject coche = GameObject.FindGameObjectWithTag("Coche");
Nodo[] trayectoria = new Nodo[3];
PID_control_hybrid pid;
float [] angulo;
float comprobar;
//Vamos a poner el siguiente punto en frente del coche
trayectoria [0] = new Nodo();
trayectoria [1] = new Nodo();
trayectoria [2] = new Nodo();
trayectoria [0].vector_hybrid = new Vector3(1.0f, 0.0f, 10.0f);
trayectoria [1].vector_hybrid = new Vector3(1.0f, 0.0f, 11.0f);
trayectoria [2].vector_hybrid = new Vector3(1.0f, 0.0f, 12.0f);
trayectoria [0].angulo_hybrid = 180.0f;
trayectoria [1].angulo_hybrid = 180.0f;
trayectoria [2].angulo_hybrid = 180.0f;
trayectoria [0].sentido = Constantes.hacia_adelante;
trayectoria [1].sentido = Constantes.hacia_adelante;
trayectoria [2].sentido = Constantes.hacia_adelante;
coche.transform.position = new Vector3(1.0f, 0.0f, 0.0f);
comprobar = coche.transform.rotation.eulerAngles.y;
if (comprobar > 180.0001f)
{
comprobar = comprobar - 360;
}
pid = new PID_control_hybrid(coche, trayectoria);
angulo = pid.pasoPID(1.0f, 0.0f, 0.0f);
Assert.IsTrue(Mathf.Approximately(angulo[1], comprobar), "1) NO son iguales: es " + angulo[1] + " | debia ser: " + comprobar);
comprobar = 50.0f; //Es la fuerza del motor que tiene que devolver si este en frente (180º)
Assert.IsTrue(Mathf.Approximately(angulo[0], comprobar), "2) NO son iguales: es " + angulo[0] + " | debia ser: " + comprobar);
}
// Se ejecuta cada vez que se calculan las fisicas. Suele ser mas de una vez por frame
void FixedUpdate()
{
if (control_manual)
{
moverCocheFisicas(input_fuerza * coche_max_torque, input_angulo * coche_max_angulo);
}
else if (encontrada_meta == true)
{
if (control_pid)
{
float[] resultado_pid;
if (a_hybrid_a_estrella)
{
resultado_pid = pid_hybrid.pasoPID(control_pid_param_p, control_pid_param_i, control_pid_param_d);
}
else
{
resultado_pid = pid.pasoPID(control_pid_param_p, control_pid_param_i, control_pid_param_d);
}
// Ha llegado a la meta y frenamos el coche
if (Mathf.Approximately(resultado_pid[0], 0.0f) && Mathf.Approximately(resultado_pid[1], 360.0f))
{
freno();
}
else // Movemos el coche
{
moverCocheFisicas(resultado_pid[0], resultado_pid[1]);
}
}
else if (contador_vector < trayectoria.Length)
{
bool llegado = false;
llegado = moverCocheSinFisicas(trayectoria [contador_vector]);
if (llegado)
{
contador_vector += 1;
}
}
}
}