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

Better wall avoidance.

Browse Source
This commit is contained in:
Wally Hackenslacker
2016-06-21 13:33:57 -04:00
parent df09ae9dfe
commit 1283ccfff1
6 changed files with 68 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
ias_robot.cpp
View File

@@ -23,28 +23,63 @@
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
*************************************************************************************/
#include <limits>
#include <sys/time.h>
#include "ias_robot.hpp"
static const long HALF_SECOND_USEC = 500000;
static const double MIN_DIST_M = 1.5;
IASSS_Robot::IASSS_Robot(std::string hostname, uint32_t port) : Robot(hostname, port) {
std::cout << "Creating IAS-SS robot on host \"" << hostname << "\" and port " << port << "." << std::endl;
}
IASSS_Robot::~IASSS_Robot() {
std::cout << "Destroying IAS-SS robot." << std::endl;
std::cout << "Destroying IAS-SS robot on " << _host_name << ":" << _port << std::endl;
}
void IASSS_Robot::run() {
int rv;
long then, now, delta, wait;
struct timeval tv;
double dist = std::numeric_limits<double>::infinity();
double dist_l = 0.0;
double dist_r = 0.0;
_p_client->Read();
if(_r_proxy->GetRange(1) < 1.0) {
if(_r_proxy->GetRange(0) > 1.0)
_p_proxy->SetSpeed(0.0f, PlayerCc::dtor(-45));
else if(_r_proxy->GetRange(2) > 1.0)
_p_proxy->SetSpeed(0.0f, PlayerCc::dtor(45));
rv = gettimeofday(&tv, NULL);
then = tv.tv_usec;
/******************************************************************************
* WALL AVOIDANCE START *
******************************************************************************/
// Check if there is something in front of the robot.
for(int i = 96; i < 126; i++)
dist = _r_proxy->GetRange(i) < dist ? _r_proxy->GetRange(i) : dist;
if(dist < MIN_DIST_M) {
for(unsigned int i = 0; i < 96; i++)
dist_r += _r_proxy->GetRange(i);
dist_r /= 96;
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(0.0f, PlayerCc::dtor(-20));
else
_p_proxy->SetSpeed(0.0f, PlayerCc::dtor(180));
} else {
_p_proxy->SetSpeed(0.3f, 0.0f);
}
sleep(1);
_p_proxy->SetSpeed(0.0f, PlayerCc::dtor(20));
} else
_p_proxy->SetSpeed(0.4f, 0.0f);
/******************************************************************************
* WALL AVOIDANCE END *
******************************************************************************/
rv = gettimeofday(&tv, NULL);
now = tv.tv_usec;
delta = now - then;
// Sleep for a bit before finishing this control iteration.
wait = rv == 0 ? HALF_SECOND_USEC - delta : HALF_SECOND_USEC;
usleep(wait);
}