sf::Color get_heatmap_color(float value)
{
static sf::Color color[] = {
sf::Color{0, 0, 128}, // 0 : deeps
sf::Color{0, 0, 255}, // 1 : shallow
sf::Color{0, 128, 255}, // 2 : shore
sf::Color{240, 240, 64}, // 3 : sand
sf::Color{17, 119, 45}, // 4 : grass
sf::Color{133, 84, 57}, // 5 : dirt
sf::Color{128, 128, 128}, // 6 : rock
sf::Color{192, 192, 192}, // 7 : mountain
sf::Color{255, 255, 255} // 8 : snow
};
if (value < -0.25f) return lerp(color[0], color[1], unlerp(-1.00f, -0.25f, value));
if (value < 0.000f) return lerp(color[1], color[2], unlerp(-0.25f, 0.000f, value));
if (value < 0.050f) return lerp(color[2], color[3], unlerp(0.000f, 0.050f, value));
if (value < 0.100f) return lerp(color[3], color[4], unlerp(0.050f, 0.100f, value));
if (value < 0.500f) return lerp(color[4], color[5], unlerp(0.100f, 0.500f, value));
if (value < 0.850f) return lerp(color[5], color[6], unlerp(0.500f, 0.850f, value));
if (value < 0.950f) return lerp(color[6], color[7], unlerp(0.850f, 0.950f, value));
return lerp(color[7], color[8], unlerp(0.950f, 1.000f, value));
}
void PathPlanner::turnToGo(const RobotPosition & robotPos, SerialCommunication & reportData) {
//take next point (X,Y) positions given by MatLab, calculate phi to turn towards, then go straight
if (currentTask == Task::IDLE) { // waiting to receive command x,y
desiredMVR = 0;
desiredMVL = 0;
if (!reportData.finished){
currentTask = Task::TURN;
turnBegin = robotPos.Phi;
if(reportData.commandIsTurn) {
turnEnd = reportData.commandPhi;
}
else {
Vector dist = reportData.commandPos - robotPos.pos;
// skip small movements
if(dist.magnitudeSq() < 0.025*0.025) currentTask = Task::DONE;
turnEnd = (reportData.commandIsReversed ? -dist : dist).angle();
}
}
}
if (currentTask == Task::TURN) { //turn towards next point
static const Spline turnSpline(0, 1, 0.3, 0.1);
// interpolate our motor speeds quadratically in angle
float through_turn = unlerp(turnBegin, turnEnd, robotPos.Phi);
float speed = sgn(turnEnd - turnBegin) * turnSpline.eval_dp(through_turn);
desiredMVR = speed;
desiredMVL = -speed;
// move on
if(through_turn >= 1 || turnBegin == turnEnd) {
desiredMVR = 0;
desiredMVL = 0;
currentTask = Task::STRAIGHT;
pathGoal = robotPos.pathDistance + (reportData.commandPos - robotPos.pos).magnitude();
// if we're just turning, this is our last step
if(reportData.commandIsTurn) currentTask = Task::DONE;
}
}
if (currentTask == Task::STRAIGHT) { //now go straight to next point
float speed = 0.4;
// slow down near the goal
const float slow_within = 0.2;
const float slow_to = 0.15;
float to_go = pathGoal - robotPos.pathDistance;
if (to_go < slow_within)
speed = lerp(slow_to, speed, to_go / slow_within);
if (to_go > 0) {
//if we aren't there yet, keep going
desiredMVR = speed;
desiredMVL = speed;
if(reportData.commandIsReversed) {
desiredMVR = -desiredMVR;
desiredMVL = -desiredMVL;
}
bool done = OrientationController(robotPos, reportData);
if(done) currentTask = Task::DONE;
} else {
currentTask = Task::DONE;
}
}
if (currentTask == Task::DONE) { //done
desiredMVR = 0;
desiredMVL = 0;
currentTask = Task::IDLE;
Serial.println("NEXT POINT");
reportData.updateStatus(true);
}
}
inline float mapAndClipRange(const float a, const float b, const float c, const float d, const float x)
{
return lerp(c, d, clamp(unlerp(a, b, x), 0.f, 1.f));
}
inline float mapRange(const float a, const float b, const float c, const float d, const float x)
{
return lerp(c, d, unlerp(a, b, x));
}
