Something went wrong. Try again.
[READ-ONLY] Mirror of https://github.com/Schniz/robotikka. Lee Ve Tom Ve Ani Doin our Robottttt
Something went wrong. Try again.
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143#include "Robot.h"#include <stdexcept>
Robot::Robot(char* ip, int port) {
_pc = new PlayerClient(ip, port); _pp = new Position2dProxy(_pc); _lp = new LaserProxy(_pc);
_pp->SetMotorEnable(true);
robot_XPos = 0; robot_YPos = 0; robot_Yaw = 0;}
Location Robot::getLocation() { return Location(this->getXPosition(), this->getYPosition(), this->getYawPosition());}
double Robot::getXPosition() { return _pp->GetXPos();}
double Robot::getYPosition() { return _pp->GetYPos();}
double Robot::getYawPosition() { return _pp->GetYaw();}
void Robot::calcLocationDeltas(double &DelX, double &DelY, double &DelYaw, AnotherMap* map) { // Getting the new position of the robot double currRobotX = Utils::MathUtil::cmToPx(_pp->GetXPos() * 100); double currRobotY = Utils::MathUtil::cmToPx(_pp->GetYPos() * 100);
// Getting the robot yaw and casting it from radians to degrees double x = _pp->GetYaw(); double currRobotYaw = x * 180 / M_PI;
// Calculating the deltas DelX = currRobotX - robot_XPos; DelY = currRobotY - robot_YPos; DelYaw = fmod(currRobotYaw - robot_Yaw, 360);
// Updating the robot location robot_XPos = currRobotX; robot_YPos = currRobotY; robot_Yaw = currRobotYaw;}
bool Robot::canTurnRight() { return canTurnInDirection(START_RIGHT_RAYS_RANGE_ANGLE, END_RIGHT_RAYS_RANGE_ANGLE);}
bool Robot::canTurnLeft() { return canTurnInDirection(START_LEFT_RAYS_RANGE_ANGLE, END_LEFT_RAYS_RANGE_ANGLE);}
bool Robot::canTurnInDirection(double startRangeAngle, double endRangeAngle) { int rangeStartIndex = angleToIndex(startRangeAngle); int rangeEndIndex = angleToIndex(endRangeAngle);
for (int i = rangeStartIndex; i < rangeEndIndex; i++) { if (getDistanceFromObstacle(i) <= 0.4) { return false; } }
return true;}
bool Robot::canMoveForward() { double dis; int rightCorner = angleToIndex(-30); int leftCorner = angleToIndex(30); for (int i = rightCorner; i <= leftCorner; i++) { dis = getDistanceFromObstacle(i); if (dis <= 0.7) { return false; } } return true;}
// This function summarize the "rays range" distances from obstaclesdouble Robot::getRaysRangeSum(double rangeStartAngle, double rangeEndAngle) { double sum = 0;
int rangeStartIndex = angleToIndex(rangeStartAngle); int rangeEndIndex = angleToIndex(rangeEndAngle);
for (int i = rangeStartIndex; i <= rangeEndIndex; i++) { sum += getDistanceFromObstacle(i); }
return sum;}
double Robot::getRangeLaser(unsigned index) { if (index < 0 || index > LASER_SLEASER_ARRAY_SIZE) { return -1; } return _lp->GetRange(index);;}
double Robot::getRangeLaser(double angle) { return getRangeLaser(angleToIndex(angle));}
double Robot::indexToAngle(int index) { double angular_resolution = LASER_FOV_DEGREE / (double) LASER_SLEASER_ARRAY_SIZE; double min_angle = LASER_FOV_DEGREE / 2;
double angle = index * angular_resolution - min_angle; return angle;}
unsigned Robot::angleToIndex(double angle) { double min_angle = LASER_FOV_DEGREE / 2;
int index = ((double) LASER_SLEASER_ARRAY_SIZE / LASER_FOV_DEGREE) * (angle + min_angle); if (index >= LASER_SLEASER_ARRAY_SIZE) { throw new out_of_range("index larger than possible."); } return index;}
double Robot::getDistanceFromObstacle(int index) { return (*_lp)[index];}
Robot::~Robot() { delete _pc; delete _pp; delete _lp;}