diff --git a/ias_robot.cpp b/ias_robot.cpp index e11e50c..aae3552 100644 --- a/ias_robot.cpp +++ b/ias_robot.cpp @@ -28,10 +28,11 @@ #include #include #include +#include #include "ias_robot.hpp" -#define PROB_MODEL_1 +//#define PROB_MODEL_1 static const float TURN_DEG_PER_SEC = 90.0f; static const float METERS_PER_SEC = 0.4f; @@ -84,7 +85,7 @@ void IASSS_Robot::run() { * WALL AVOIDANCE START * ******************************************************************************/ // Check if there is something in front of the robot. - for(int i = 96; i < 126; i++) + for(int i = 89; i < 134; i++) dist = _r_proxy->GetRange(i) < dist ? _r_proxy->GetRange(i) : dist; if(dist < MIN_DIST_M && dist > CRIT_DIST_M) { @@ -133,6 +134,7 @@ float IASSS_Robot::brss() { std::map U, V; unsigned int i_min, i_max; float min, sample, prob, max, sum_uv = 0.0f, steer; + std::ostringstream oss; while(U.size() < (U_RATIO * NUM_PHERO_SAMPLES)) { min = std::numeric_limits::max(); @@ -198,7 +200,7 @@ float IASSS_Robot::brss() { #ifdef PROB_MODEL_1 _phero_sensor.probs[i] = 1.0f / (_phero_sensor[i] / sum_uv); #else - _phero_sensor.probs[i] = (1.0f - _phero_sensor[i]) / ( / sum_uv); + _phero_sensor.probs[i] = (1.0f - _phero_sensor[i]) / (sum_uv); #endif } @@ -219,5 +221,12 @@ float IASSS_Robot::brss() { steer = (NUM_PHERO_SAMPLES / 2.0f) - i_max; + oss << "samples: " << std::endl; + for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) + oss << "\tSAMPLE[" << i << "]: " << _phero_sensor[i] << " - " << _phero_sensor.sample_amnt[i] << " - " << _phero_sensor.probs[i] << std::endl; + oss << "\ti_max: " << i_max << " | Steer: " << steer; + + log(oss.str()); + return steer; } diff --git a/pheromone.cpp b/pheromone.cpp index 89a3b3b..ce8e710 100644 --- a/pheromone.cpp +++ b/pheromone.cpp @@ -187,52 +187,51 @@ void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw float cos_theta; float ang; glm::vec2 v, vp; - glm::vec3 line, v3d; if(sensor == NULL) return; + else { + sensor->reset(); + // Calculate the robot's view vector. v = glm::vec2(radius * cos(yaw), radius * sin(yaw)) - glm::vec2(0.0, 0.0); v = glm::normalize(v); sem_wait(&map_semaphore); { + // For every point in the pheromone map. for(unsigned i = 0; i < m_height; i++) { for(unsigned j = 0; j < m_width; j++) { - vp = glm::vec2(j/float(m_width), 1.0f - (i/float(m_height))) - glm::vec2(x, y); + // Calculate the vector from the robot's center to the sampling point. + vp = glm::vec2(j / float(m_width), 1.0f - (i / float(m_height))) - glm::vec2(x, y); + // Distance from the sampling point to the robot's center. dist = glm::length(vp); vp = glm::normalize(vp); + // Cosine of the angle between the robot's center and the sampling point. cos_theta = glm::dot(vp, v); + // Check if the point is close enough to the front of the robot. if(cos_theta > 0.0f && dist <= radius) { cos_theta = cos_theta > 1.0f ? 1.0f : cos_theta; + // Get the angle between the robot's center and the sampling point. ang = PlayerCc::rtod(acos(cos_theta)); - line = glm::vec3(v.x, v.y, 0.0f); - v3d = glm::vec3(vp.x, vp.y, 0.0f); - if(side(line, v3d) > 0) { - index = static_cast((NUM_PHERO_SAMPLES / 2.0f) - ang); - index = index >= NUM_PHERO_SAMPLES ? NUM_PHERO_SAMPLES - 1 : index; - if(MAP_POS(i, j) > sensor->samples[index]) { - sensor->samples[index] = MAP_POS(i, j); - sensor->sample_amnt[index] += 1; - } - } else { - index = static_cast((NUM_PHERO_SAMPLES / 2.0f) + ang); - index = index >= NUM_PHERO_SAMPLES ? NUM_PHERO_SAMPLES - 1 : index; - if(MAP_POS(i, j) > sensor->samples[index]) { - sensor->samples[index] = MAP_POS(i, j); - sensor->sample_amnt[index] += 1; - } - } + + // Put the sample in the sampling pool. + index = static_cast(ang / (180 / 5)); + index = index >= NUM_PHERO_SAMPLES ? NUM_PHERO_SAMPLES - 1 : index; + sensor->samples[index] = MAP_POS(i, j); + sensor->sample_amnt[index] += 1; + } else continue; } } } sem_post(&map_semaphore); - /*for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) { + // Average the samples. + for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) { sensor->samples[i] = (sensor->sample_amnt[i] > 0) ? (sensor->samples[i] / sensor->sample_amnt[i]) : 0.0f; - }*/ + } } } diff --git a/pheromone.hpp b/pheromone.hpp index 01eb95b..d9ca139 100644 --- a/pheromone.hpp +++ b/pheromone.hpp @@ -34,7 +34,7 @@ extern const unsigned char MAX_PHERO_INTENSITY; extern const unsigned char MIN_PHERO_INTENSITY; -const unsigned int NUM_PHERO_SAMPLES = 180; +const unsigned int NUM_PHERO_SAMPLES = 5; typedef struct PHERO_SENSOR { float samples[NUM_PHERO_SAMPLES];