public void anguloGiroHybridTest()
{
GameObject coche = GameObject.FindGameObjectWithTag("Coche");
Nodo[] trayectoria = new Nodo[3];
PID_control_hybrid pid;
float angulo;
float comprobar;
trayectoria [0] = new Nodo();
trayectoria [1] = new Nodo();
trayectoria [2] = new Nodo();
trayectoria [0].vector_hybrid = new Vector3(45.0f, 0.0f, 0.0f);
trayectoria [1].vector_hybrid = new Vector3(45.0f, 0.0f, 1.0f);
trayectoria [2].vector_hybrid = new Vector3(45.0f, 0.0f, 2.0f);
coche.transform.position = new Vector3(1.0f, 0.0f, 0.0f);
Vector3 prueba = new Vector3(0.0f, 0.0f, 1.0f);
comprobar = coche.transform.rotation.eulerAngles.y;
if (comprobar > 180.0001f)
{
comprobar = comprobar - 360;
}
pid = new PID_control_hybrid(coche, trayectoria);
angulo = pid.anguloGiro(prueba, Constantes.hacia_adelante);
Assert.IsTrue(Mathf.Approximately(angulo, comprobar), "NO son iguales: es " + angulo + " | debia ser: " + comprobar);
}
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 una vez cada frame
void Update()
{
bool fin;
// Solo lo usamos en caso de que se seleccione el control manual
input_fuerza = Input.GetAxis("Vertical");
input_angulo = Input.GetAxis("Horizontal");
if (error) // No se ha encontrado una ruta hasta la meta
{
Debug.Log("Error: no se ha encontrado un camino");
parrilla.borrarTodasCasillas();
}
else if (!encontrada_meta)
{
// Se ejecuta en cada update hasta que se encuentre la meta
fin = script_algoritmo.pasoCalcularRuta(out error);
if (fin) //Se ha encontrado la meta
{
final = Time.realtimeSinceStartup;
encontrada_meta = true;
Debug.Log("Ruta calculada en: " + (final - inicio));
parrilla.borrarTodasCasillas();
trayectoria = script_algoritmo.getTrayectoria();
trayectoria_nodos = script_algoritmo.getTrayectoriaNodos();
if (ps_path_smoothing_activado)
{
Vector3[] trayectoria_ps = trayectoria;
if (a_hybrid_a_estrella)
{
trayectoria_ps = trayectoriaHybrid(trayectoria_nodos, trayectoria_nodos.Length);
}
path_smoothing = new PathSmoothing(mapa, trayectoria_ps, ps_peso_trayectoria, ps_peso_suavizado);
if (ps_bezier)
{
trayectoria = path_smoothing.getTrayectoriaSuavizadaCurvasBezier();
}
else if (ps_descenso_gradiente)
{
trayectoria = path_smoothing.getTrayectoriaDescensoGradiente();
}
else
{
trayectoria = path_smoothing.eliminarZigZag(trayectoria);
}
}
contador_vector = 0;
if (a_hybrid_a_estrella)
{
DibujarTrayectoria(trayectoria_nodos, trayectoria_nodos.Length);
}
else
{
DibujarTrayectoria(trayectoria, trayectoria.Length);
}
pid = new PID_control(coche, trayectoria);
pid_hybrid = new PID_control_hybrid(coche, trayectoria_nodos);
pid.setParrilla(parrilla);
pid_hybrid.setParrilla(parrilla);
}
}
}
