WallyHackenslacker/IAS-SS
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Almost there...

Browse Source
This commit is contained in:
Wally Hackenslacker
2016-06-27 16:15:10 -04:00
parent fdfe955787
commit cecd37420d
7 changed files with 79 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
ant.world
View File

@@ -42,27 +42,27 @@ floorplan
ant_bot
(
name "ant_robot_1"
pose [ -4.044 7.232 0.000 5.000 ]
pose [ -6.432 -5.895 0 45.000 ]
color "green"
)
ant_bot
(
name "ant_robot_2"
pose [ -5.443 -5.199 0.000 111.000 ]
pose [ 6.432 -3.895 0 135.000 ]
color "green"
)
ant_bot
(
name "ant_robot_3"
pose [ -3.915 3.631 0.000 151.000 ]
pose [ -6.432 5.895 0 -45.000 ]
color "green"
)
ant_bot
(
name "ant_robot_4"
pose [ -0.310 5.411 0.000 77.000 ]
pose [ 6.432 5.895 0 -135.000 ]
color "green"
)

16
ias_robot.cpp
View File

@@ -36,7 +36,7 @@ static const long HALF_SECOND_USEC = 500000;
static const double MIN_DIST_M = 1.5;
static const double CRIT_DIST_M = 1.0;
static const float MAP_SIZE = 16.0f;
static const int PHERO_AMOUNT = 10;
static const int PHERO_AMOUNT = 25;
static const float PHERO_RADIUS = 1.0;
static const float SENSOR_RADIUS = 2.0;
@@ -59,13 +59,17 @@ void IASSS_Robot::run() {
int rv;
long then, now, delta, wait;
struct timeval tv;
double dist = std::numeric_limits<double>::infinity();
double dist = std::numeric_limits<double>::infinity();
_p_client->Read();
rv = gettimeofday(&tv, NULL);
then = tv.tv_usec;
x = (_p_proxy->GetXPos() + (MAP_SIZE / 2)) / MAP_SIZE;
y = (_p_proxy->GetYPos() + (MAP_SIZE / 2)) / MAP_SIZE;
_phero_map->s_sample(&_phero_sensor, x, y, _p_proxy->GetYaw(), SENSOR_RADIUS / MAP_SIZE);
/******************************************************************************
* WALL AVOIDANCE START *
******************************************************************************/
@@ -75,17 +79,13 @@ void IASSS_Robot::run() {
if(dist < MIN_DIST_M && dist > CRIT_DIST_M) {
avoid_wall(METERS_PER_SEC, TURN_DEG_PER_SEC);
} else if(dist < CRIT_DIST_M) {
} else if(dist <= CRIT_DIST_M) {
avoid_wall(0.0f, TURN_DEG_PER_SEC);
} else
_p_proxy->SetSpeed(METERS_PER_SEC, 0.0f);
/******************************************************************************
* WALL AVOIDANCE END *
******************************************************************************/
x = (_p_proxy->GetXPos() + (MAP_SIZE / 2)) / MAP_SIZE;
y = (_p_proxy->GetYPos() + (MAP_SIZE / 2)) / MAP_SIZE;
_phero_map->s_sample(&_phero_sensor, x, y, _p_proxy->GetYaw(), SENSOR_RADIUS / MAP_SIZE);
deposit_pheromone();
@@ -109,7 +109,7 @@ void IASSS_Robot::avoid_wall(float front_speed, float turn_speed) {
for(unsigned int i = 126; i < _r_proxy->GetRangeCount(); i++)
dist_l += _r_proxy->GetRange(i);
dist_l /= (_r_proxy->GetRangeCount() - 126);
if(dist_r >= dist_l)
_p_proxy->SetSpeed(front_speed, PlayerCc::dtor(-turn_speed));
else

BIN
maps/cave_mask.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 911 B

After

Width:  |  Height:  |  Size: 953 B

BIN
maps/cave_mask_copy.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 911 B

56
pheromone.cpp
View File

@@ -26,6 +26,7 @@
#include <iostream>
#include <cstdlib>
#include <cmath>
#include <cassert>
#include <pnglite.h>
#include <glm/glm.hpp>
@@ -33,10 +34,18 @@
#define MAP_POS(X, Y) (data[((X) * m_height) + (Y)])
static const float EVAPORATION_RATE = 0.1f;
const unsigned char MAX_PHERO_INTENSITY = 250;
static const float EVAPORATION_RATE = 0.05f;
const unsigned char MAX_PHERO_INTENSITY = 255;
const unsigned char MIN_PHERO_INTENSITY = 1;
static inline int sign(float f) {
return (f < 0.0f) ? -1 : ((f > 0.0f) ? 1 : 0);
}
static inline int side(glm::vec3 line, glm::vec3 v) {
return sign(glm::cross(line, v).z);
}
PheromoneMap::PheromoneMap(const char * file_name) {
load_map(file_name);
sem_init(&map_semaphore, 0, 1);
@@ -99,12 +108,11 @@ bool PheromoneMap::s_deposit_pheromone(float x, float y) {
if((y > 1.0 || y < 0.0) || (x > 1.0 || x < 0.0))
return false;
if((_y >= m_height || _y < 0) || (_x >= m_width || _x < 0))
return false;
sem_wait(&map_semaphore); {
if((_y >= m_height || _y < 0) || (_x >= m_width || _x < 0))
return false;
if(MAP_POS(_y, _x) <= MAX_PHERO_INTENSITY) {
if(MAP_POS(_y, _x) >= MIN_PHERO_INTENSITY) {
MAP_POS(_y, _x) = MAX_PHERO_INTENSITY;
valid = true;
}
@@ -125,7 +133,7 @@ void PheromoneMap::s_evaporate() {
sem_wait(&map_semaphore); {
for(unsigned i = 1; i < m_height - 1; i++) {
for(unsigned j = 1; j < m_width - 1; j++) {
if(MAP_POS(i, j) >= MIN_PHERO_INTENSITY && MAP_POS(i, j) <= MAX_PHERO_INTENSITY) {
if(MAP_POS(i, j) >= MIN_PHERO_INTENSITY) {
p_eva = MAP_POS(i, j) * EVAPORATION_RATE;
MAP_POS(i, j) -= p_eva;
}
@@ -135,18 +143,22 @@ void PheromoneMap::s_evaporate() {
}
void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw, float radius) {
float _x = x;
float _y = y;
float _r = radius;
float _x = x;
float _y = y;
float dist;
float cos_theta;
float ang;
glm::vec2 v;
glm::vec2 vp;
glm::vec3 line;
glm::vec3 v3d;
if(sensor == NULL)
return;
else {
v = glm::vec2(_r * cos(yaw), _r * sin(yaw)) - glm::vec2(0.0, 0.0);
sensor->reset();
v = glm::vec2(radius * cos(yaw), radius * sin(yaw)) - glm::vec2(0.0, 0.0);
v = glm::normalize(v);
sem_wait(&map_semaphore); {
@@ -157,14 +169,28 @@ void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw
vp = glm::normalize(vp);
cos_theta = glm::dot(vp, v);
if(cos_theta > 0.0f && dist <= _r) {
if(MAP_POS(i, j) >= MIN_PHERO_INTENSITY && MAP_POS(i, j) <= MAX_PHERO_INTENSITY) {
MAP_POS(i, j) = MAX_PHERO_INTENSITY;
if(cos_theta > 0.0f && dist <= radius) {
ang = 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) {
assert(static_cast<unsigned int>(90.0f - ang) <= 90);
sensor->samples[static_cast<unsigned int>(90.0f - ang)] += MAP_POS(i, j);
sensor->sample_amnt[static_cast<unsigned int>(90.0f - ang)] += 1;
//MAP_POS(i, j) = MAX_PHERO_INTENSITY;
} else {
assert(static_cast<unsigned int>(90.0f + ang) < 180);
sensor->samples[static_cast<unsigned int>(90.0f + ang)] += MAP_POS(i, j);
sensor->sample_amnt[static_cast<unsigned int>(90.0f + ang)] += 1;
//MAP_POS(i, j) = MAX_PHERO_INTENSITY;
}
} else
continue;
}
}
} sem_post(&map_semaphore);
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;
}
}

27
pheromone.hpp
View File

@@ -37,17 +37,38 @@ extern const unsigned char MIN_PHERO_INTENSITY;
const unsigned int NUM_PHERO_SAMPLES = 180;
typedef struct PHERO_SENSOR {
float samples[NUM_PHERO_SAMPLES];
float samples[NUM_PHERO_SAMPLES];
unsigned int sample_amnt[NUM_PHERO_SAMPLES];
float probs[NUM_PHERO_SAMPLES];
PHERO_SENSOR() {
memset(this->samples, 0.0f, NUM_PHERO_SAMPLES);
reset();
}
void reset() {
memset(sample_amnt, 0, sizeof(unsigned int) * NUM_PHERO_SAMPLES);
for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) {
samples[i] = 0.0f;
probs[i] = 0.0f;
}
}
void set_probabilities() {
float phero_sum = 0.0f;
for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++)
phero_sum += samples[i];
for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) {
probs[i] = 1.0f / (samples[i] / phero_sum);
}
}
float operator[](unsigned int index) {
if(index >= NUM_PHERO_SAMPLES)
return -1.0f;
else
return this->samples[index];
return samples[index];
}
} phero_sensor_t;

3
shaders/basic.frag
View File

@@ -4,5 +4,6 @@ uniform sampler2D sTexture;
void main() {
vec4 tex = texture2D(sTexture, gl_TexCoord[0].st);
gl_FragColor = clamp(tex, 0.0, 1.0);
vec4 color = mix(vec4(0.0, 0.0, 0.3, 1.0), vec4(1.0, 1.0, 0.0, 1.0), tex.r);
gl_FragColor = clamp(color, 0.0, 1.0);
}