public void pasoPIDTest() { GameObject coche = GameObject.FindGameObjectWithTag("Coche"); Vector3[] trayectoria = new Vector3[3]; PID_control pid; float [] angulo; float comprobar; //Vamos a poner el siguiente punto en frente del coche trayectoria [0] = new Vector3(1.0f, 0.0f, 10.0f); trayectoria [1] = new Vector3(1.0f, 0.0f, 11.0f); trayectoria [2] = new Vector3(1.0f, 0.0f, 12.0f); 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(coche, trayectoria); angulo = pid.pasoPID(1.0f, 0.0f, 0.0f); Assert.IsTrue(Mathf.Approximately(angulo[1], comprobar), "NO son iguales: es " + angulo[1] + " | debia ser: " + comprobar); comprobar = 130.0f; //Es la fuerza del motor que tiene que devolver si este en frente (180º) Assert.IsTrue(Mathf.Approximately(angulo[0], comprobar), "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; } } } }