float getMotorPower(float direction, int wheelOri) {
float finPower;
float tempDir;
if (wheelOri == LEFT_ORIENTATION_HOLO_WHEEL) {
tempDir = modAngle(direction - 45);
} else if (wheelOri == RIGHT_ORIENTATION_HOLO_WHEEL) {
tempDir = modAngle(direction + 45);
}
finPower = getRefAngle(tempDir)/90*100;
if (tempDir > 180) {
finPower = -finPower;
}
return finPower;
}
double Entity::getAngle(double dx, double dy) {
double angle;
if (dx != 0 || dy != 0) {
double theta = atan2(-dx, dy);
angle = theta * 180 / M_PI + 180;
angle = modAngle(angle);
} else {
esTools::Debug("I: DEBUG: Test: x = " + std::to_string(dx) + " y = " + std::to_string(dy));
esTools::Debug("W: DEBUG: Moveable::getAngle: Getting angle with a zero vector!");
angle = 0;
}
return angle;
}
// Given a point within the Cartesian coordinate system, this function will calculate the angle, in standard position, from the origin to the given point.
float getAngle(float x, float y) {
float angle = 0;
float refAngle = 0;
//int quadrant = 0;
// Find angle in Standard Position
if (x != 0 && y != 0) { // Exclude any Quadrantal Angles
// Find Reference Angle
refAngle = atan(y/x)*180/PI;
// Find Angle in Standard Position
if (x < 0) { // Quadrant II or III
angle = 180 + refAngle;
} else if (x > 0) { // Quadrant IV or I
angle = 360 + refAngle;
}
// Ensure angle is within bounds: [0, 360)
angle = modAngle(angle);
}
// Quadrantal Angles
if (x == 0) {
if (y == 0) {
angle = 0; // Default to 0 degrees if point == origin
} else if (y < 0) {
angle = 270;
} else if (y > 0) {
angle = 90;
}
} else if (y == 0) {
if (x < 0) {
angle = 180;
} else if (x > 0) {
angle = 0;
}
}
// Old Algorithm
/*
if (x != 0 && y != 0) { // Exclude any Quadrantal Angles
// Find Reference Angle
refAngle = atan(y/x);
// Find Angle in Standard Position
if (x < 0) { // Quadrant II or III
if (refAngle < 0) { // Quadrant II or IV therefore Quadrant II
angle = 180 + refAngle;
} else if (refAngle > 0) { // Quadrant I or III therefore Quadrant III
angle = 180 + refAngle;
}
} else if (x > 0) { // Quadrant IV or I
if (refAngle < 0) { // Quadrant II or IV therefore Quadrant IV
angle = 360 + refAngle;
} else if (refAngle > 0) { // Quadrant I or III therefore Quadrant I
angle = refAngle;
}
}
}
// Find Quadrant
if (x < 0) {
if (y < 0) {
quadrant = 3;
} else if (y > 0) {
quadrant = 2;
}
} else if (x > 0) {
if (y < 0) {
quadrant = 4;
} else if (y > 0) {
quadrant = 1;
}
}
*/
return angle;
}
