示例#1
0
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));
}
示例#2
0
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);
  }
}
示例#3
0
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));
}
示例#4
0
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));
}