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); }