void dibujarMapaObstaculos(int [,,] mapa_obstaculos)
{
int inicio_ancho = (ancho / 2) * (-1);
int inicio_largo = (largo / 2) * (-1);
for (int i = inicio_ancho; i <= (ancho / 2); i++)
{
for (int j = inicio_largo; j <= (largo / 2); j++)
{
Vector3 n = new Vector3(i, 0.0f, j);
parrilla.crearCasilla(n, mapa_obstaculos[i + (ancho / 2), j + (largo / 2), 0]);
}
}
}
public void setParrilla(Parrilla p_parrilla)
{
parrilla = p_parrilla;
parrilla.crearCasilla(trayectoria[punto_actual].vector_hybrid, 0);
}
public float[] pasoPID(float parametro_p, float parametro_i, float parametro_d)
{
float distancia = 0.0f;
float fuerza_motor = 0.0f;
float angulo_giro = 0.0f;
float[] retorno = new float[2];
float angulo_error = 0.0f;
float diferencial_error;
float error_interno = 0.0f;
Vector3 destino;
float distancia_cambio = 1.9f;
bool ya_cambiado = false;
if (!fin)
{
if (sentido_anterior != trayectoria [punto_actual].sentido)
{
cambio_sentido = true;
}
if (cambio_sentido)
{
if (rb_coche.velocity.magnitude > 0.0001f)
{
fuerza_motor = 0.0f;
angulo_giro = 0.0f;
}
else
{
cambio_sentido = false;
sentido_anterior = trayectoria [punto_actual].sentido;
}
}
else
{
if (punto_actual < punto_meta)
{
if (trayectoria [punto_actual].sentido == Constantes.hacia_adelante)
{
//distancia = Vector3.Distance (eje_trasero.transform.position, trayectoria [punto_actual].vector_hybrid);
distancia = Vector3.Distance(eje_delantero.transform.position, trayectoria [punto_actual].vector_hybrid);
destino = trayectoria [punto_actual].vector_hybrid - eje_trasero.transform.position;
}
else if (trayectoria [punto_actual].sentido != trayectoria [punto_actual + 1].sentido)
{
//distancia = Vector3.Distance (eje_trasero.transform.position, trayectoria [punto_actual].vector_hybrid);
distancia = Vector3.Distance(eje_delantero.transform.position, trayectoria [punto_actual].vector_hybrid);
destino = trayectoria [punto_actual].vector_hybrid - eje_delantero.transform.position;
}
else
{
distancia = Vector3.Distance(eje_trasero.transform.position, trayectoria [punto_actual].vector_hybrid);
//distancia = Vector3.Distance (eje_delantero.transform.position, trayectoria [punto_actual].vector_hybrid);
destino = trayectoria [punto_actual].vector_hybrid - eje_delantero.transform.position;
}
}
else
{
if (trayectoria [punto_actual].sentido == Constantes.hacia_adelante)
{
//distancia = Vector3.Distance (eje_trasero.transform.position, trayectoria [punto_actual].vector_hybrid);
distancia = Vector3.Distance(eje_delantero.transform.position, trayectoria [punto_actual].vector_hybrid);
destino = trayectoria [punto_actual].vector_hybrid - eje_trasero.transform.position;
}
else
{
distancia = Vector3.Distance(eje_trasero.transform.position, trayectoria [punto_actual].vector_hybrid);
//distancia = Vector3.Distance (eje_delantero.transform.position, trayectoria [punto_actual].vector_hybrid);
destino = trayectoria [punto_actual].vector_hybrid - eje_delantero.transform.position;
}
}
if (punto_actual == punto_meta)
{
distancia_cambio = 1.6f;
}
else
{
if (trayectoria [punto_actual].sentido == Constantes.hacia_adelante)
{
distancia_cambio = 1.5f;
}
else
{
distancia_cambio = 0.2f;
}
}
if (distancia > distancia_cambio) //Si la distancia al punto es pequeña pasamos al siguiente
{
angulo_error = anguloGiro(destino, trayectoria [punto_actual].sentido);
//angulo_error = coche.transform.rotation.eulerAngles.y - trayectoria[punto_actual].angulo_hybrid;
//angulo_error = trayectoria[punto_actual].angulo_hybrid - coche.transform.rotation.eulerAngles.y;
diferencial_error = angulo_error - error_anterior;
error_anterior = angulo_error;
error_interno += angulo_error;
angulo_giro = (angulo_error * parametro_p) - (diferencial_error * parametro_d) - (error_interno * parametro_i);
fuerza_motor = (20.0f * distancia) + (0.5f * Mathf.Abs(angulo_giro));
if (Mathf.Abs(fuerza_motor) > 50.0f)
{
fuerza_motor = 50.0f; // + (0.5f * Mathf.Abs (angulo_giro));
}
if (trayectoria [punto_actual].sentido == Constantes.hacia_atras)
{
fuerza_motor *= trayectoria [punto_actual].sentido;
fuerza_motor *= 0.5f; //Mas despacio por ir hacia atras
angulo_giro *= trayectoria [punto_actual].sentido;
}
}
else
{
if (punto_actual == punto_meta) //Si hemos llegado a la meta
{
fuerza_motor = 0.0f;
angulo_giro = 360.0f;
fin = true;
Debug.Log("Destino alcanzado");
}
else //Si estamos en un destino parcial pero aun no hemos llegado a la meta
{
punto_actual++;
ya_cambiado = true;
distancia_anterior = 9999.9f;
parrilla.crearCasilla(trayectoria [punto_actual].vector_hybrid, Constantes._ABIERTOS);
}
}
if (distancia_anterior < (distancia - 0.0005f) && !fin)
{
punto_actual++;
distancia_anterior = 9999.9f;
parrilla.crearCasilla(trayectoria [punto_actual].vector_hybrid, Constantes._ABIERTOS);
}
else
{
if (!ya_cambiado)
{
distancia_anterior = distancia;
}
else
{
ya_cambiado = false;
}
}
}
if (punto_actual > punto_meta)
{
punto_actual = punto_meta;
}
}
else
{
fuerza_motor = 0.0f;
angulo_giro = 360.0f;
}
retorno [0] = fuerza_motor;
retorno [1] = angulo_giro;
return(retorno);
}
public float[] pasoPID(float parametro_p, float parametro_i, float parametro_d)
{
float distancia = 0.0f;
//float distancia2 = 0.0f;
float fuerza_motor = 0.0f;
float angulo_giro = 0.0f;
float[] retorno = new float[2];
float angulo_error;
float diferencial_error;
float error_interno = 0.0f;
Vector3 destino;
float distancia_cambio = 1.9f;
if (!fin)
{
distancia = Vector3.Distance(eje_delantero.transform.position, trayectoria [punto_actual]);
destino = trayectoria [punto_actual] - eje_trasero.transform.position;
if (punto_actual == punto_meta)
{
distancia_cambio = 2.9f;
}
else
{
distancia_cambio = 1.9f;
}
if (distancia > distancia_cambio) //Si la distancia al punto es pequeña pasamos al siguiente
{
angulo_error = anguloGiro(destino);
diferencial_error = angulo_error - error_anterior;
error_anterior = angulo_error;
error_interno += angulo_error;
angulo_giro = (angulo_error * parametro_p) - (diferencial_error * parametro_d) - (error_interno * parametro_i);
fuerza_motor = (7.0f * distancia) + (0.5f * Mathf.Abs(angulo_giro));
if (fuerza_motor > 40.0f)
{
fuerza_motor = 40.0f + (0.5f * Mathf.Abs(angulo_giro));
}
}
else
{
if (punto_actual == punto_meta) //Si hemos llegado a la meta
{
fuerza_motor = 0.0f;
angulo_giro = 360.0f;
Debug.Log("Destino alcanzado");
fin = true;
}
else //Si estamos en un destino parcial pero aun no hemos llegado a la meta
{
punto_actual++;
parrilla.crearCasilla(trayectoria[punto_actual], 0);
}
}
}
else
{
fuerza_motor = 0.0f;
angulo_giro = 360.0f;
}
retorno [0] = fuerza_motor;
retorno [1] = angulo_giro;
return(retorno);
}
