Skip to content

Instantly share code, notes, and snippets.

@selimslab selimslab/robot.cpp
Created May 23, 2020

Embed
What would you like to do?
a mobile robot. obstacle avoiding, path-finder. built using Raspberry Pi, ROS, C++, and A* algorithm
#include <iostream>
#include <tuple>
#include <cmath>
const double PI = 3.141592653589793238463;
using namespace std;
class RobotController {
float robotRadius = 0.1;
float obstacleRadius = 0.2;
float vMax = 0.2;
float wMax = PI / 2;
float epsilon = 0.01;
int initial_degree = 0;
float axleLength = 0.2;
float wheelRadius = 0.045;
float wheelCircumference = 2 * PI * wheelRadius;
float topViewRobotCircumference = PI * axleLength;
float obstaclePositions[3][3];
float robotPosition[3] = { 0, 0, 0 };
float robotOrientation[3];
bool leftCameraDetectedObstacle;
float distanceLeft;
float leftPosition[];
bool rightCameraDetectedObstacle;
float distanceRight;
float rightPosition[];
tuple<float, float> subtract_arrays(float x[], float y[])
{
float dx = x[0] - y[0];
float dy = x[1] - y[1];
return make_tuple(dx, dy);
}
float euclidean_distance(float dx, float dy)
{
return dx * dx + dy * dy;
}
float get_distance(float currentPosition[], float distantPosition[])
{
float dx, dy;
tie(dx, dy) = subtract_arrays(currentPosition, distantPosition);
float distance = euclidean_distance(dx, dy);
return distance;
}
public:
void go()
{
bool is_first_obstacle = true;
if (initial_degree < 360) {
setRobotSpeed(0, 3);
initial_degree = initial_degree + 6;
if (leftCameraDetectedObstacle == true and is_first_obstacle == true) {
obstaclePositions[0][0] = leftPosition[0] + robotPosition[0];
obstaclePositions[0][1] = leftPosition[1] + robotPosition[1];
obstaclePositions[0][2] = leftPosition[2] + robotPosition[2];
is_first_obstacle = false;
}
}
else {
float robotTheta = robotOrientation[3] + PI / 2;
// Get from cameras
float goalPosition[3];
float obstaclePositions[3][3];
float distance_to_goal = get_distance(robotPosition, goalPosition);
float position_of_middle_of_obstacles[3];
float distance_from_middle_of_obstacles = get_distance(robotPosition, position_of_middle_of_obstacles);
bool any_obstacle = true;
float Fx, Fy;
if (any_obstacle == true) {
tie(Fx, Fy) = calculateGradient(position_of_middle_of_obstacles, robotPosition, obstaclePositions);
}
else {
tie(Fx, Fy) = calculateGradient(goalPosition, robotPosition, obstaclePositions);
}
float v = 0;
float w = 0;
float Fmag = sqrt(Fx * Fx + Fy * Fy);
float Fth = atan2(Fy, Fx);
float th = Fth - robotTheta;
v = vMax * cos(th);
w = wMax * sin(th);
if (distance_from_middle_of_obstacles < epsilon) {
any_obstacle = false;
}
if (distance_to_goal < epsilon) {
setRobotSpeed(0, 0);
}
else {
setRobotSpeed(v, w);
}
}
}
bool sign(int x)
{
return (x > 0 and 1) or (x < 0 and -1) or 0;
}
tuple<float, float> calculateGradient(float goalPosition[], float robotPosition[], float obstaclePositions[3][3])
{
float Fx = 0;
float Fy = 0;
float dgx, dgy, do1x, do1y, do2x, do2y, do3x, do3y;
tie(dgx, dgy) = subtract_arrays(robotPosition, goalPosition);
tie(do1x, do1y) = subtract_arrays(robotPosition, obstaclePositions[0]);
tie(do2x, do2y) = subtract_arrays(robotPosition, obstaclePositions[1]);
tie(do3x, do3y) = subtract_arrays(robotPosition, obstaclePositions[2]);
float gamma = dgx * dgx + dgy * dgy;
float B1 = do1x * do1x + do1y * do1y - (robotRadius + obstacleRadius) * (robotRadius + obstacleRadius);
float B2 = do2x * do2x + do2y * do2y - (robotRadius + obstacleRadius) * (robotRadius + obstacleRadius);
float B3 = do3x * do3x + do3y * do3y - (robotRadius + obstacleRadius) * (robotRadius + obstacleRadius);
float B = B1 * B2 * B3;
int k = 5;
Fx = (k * pow(gamma, k - 1) * 2 * dgx * B - pow(gamma, k) * (2 * do1x * B2 * B3 + 2 * do2x * B1 * B3 + 2 * do3x * B2 * B1)) / (B * B);
Fy = (k * pow(gamma, k - 1) * 2 * dgy * B - pow(gamma, k) * (2 * do1y * B2 * B3 + 2 * do2y * B1 * B3 + 2 * do3y * B2 * B1)) / (B * B);
return make_tuple(-Fx, -Fy);
}
void setRobotSpeed(float transVel, float rotVel)
{
// Convert speed to rad/sec
transVel = transVel / wheelCircumference * 2 * PI;
rotVel = rotVel * (topViewRobotCircumference / wheelCircumference);
// Give speed to both left and right motor
float leftMotorSpeed = transVel - rotVel;
float rightMotorSpeed = -transVel - rotVel;
int tau_max = 5;
if (abs(leftMotorSpeed) > tau_max) {
leftMotorSpeed = sign(leftMotorSpeed) * tau_max;
}
if (abs(rightMotorSpeed) > tau_max) {
rightMotorSpeed = sign(rightMotorSpeed) * tau_max;
}
}
};
int main()
{
std::cout << "Hello World!\n";
RobotController controller = RobotController();
controller.go();
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.