Example #1
0
void Robot::handleTasks(float dt) {
  Task *task = getCurrentTask();

  if (task == NULL)
    return;

  if (!task->isStarted()) {
    task->onStart(*this, dt);

    task->setStarted(true);
  }

  if (task->onStep(*this, dt) == false) {
    task->onEnd(*this, dt);

    delete task;

    tasks.pop_front();

    handleTasks(dt);
  }
}
Example #2
0
void Robot::step(float dt, Vision::Results *visionResults) {
  this->visionResults = visionResults;

  lastDt = dt;
  totalTime += dt;

  if (!coilgunCharged) {
    coilgun->charge();

    coilgunCharged = true;
  }

  handleTasks(dt);
  updateWheelSpeeds();
  updateBallLocalizer(visionResults, dt);

  wheelFL->step(dt);
  wheelFR->step(dt);
  wheelRL->step(dt);
  wheelRR->step(dt);
  coilgun->step(dt);
  dribbler->step(dt);

  com->send("speeds:"
            + Util::toString((int)Math::round(wheelFL->getTargetSpeed())) + ":"
            + Util::toString((int)Math::round(wheelFR->getTargetSpeed())) + ":"
            + Util::toString((int)Math::round(wheelRL->getTargetSpeed())) + ":"
            + Util::toString((int)Math::round(wheelRR->getTargetSpeed())) + ":"
            + Util::toString((int)Math::round(dribbler->getTargetSpeed()))
           );

  movement = odometer->calculateMovement(
               wheelFL->getRealOmega(),
               wheelFR->getRealOmega(),
               wheelRL->getRealOmega(),
               wheelRR->getRealOmega()
             );

  Math::Vector velocityVec(movement.velocityX, movement.velocityY);
  lastVelocity = velocity;
  velocity = velocityVec.getLength();

  omega = movement.omega;

  travelledDistance += velocity * dt;
  travelledRotation += movement.omega * dt;

  updateMeasurements();
  updateBallLocalizer(visionResults, dt);

  robotLocalizer->update(measurements);
  robotLocalizer->move(movement.velocityX, movement.velocityY, movement.omega, dt, measurements.size() == 0 ? true : false);
  odometerLocalizer->move(movement.velocityX, movement.velocityY, movement.omega, dt);

  Math::Position localizerPosition = robotLocalizer->getPosition();
  Math::Position odometerPosition = odometerLocalizer->getPosition();

  x = localizerPosition.x;
  y = localizerPosition.y;
  orientation = localizerPosition.orientation;

  std::stringstream stream;

  stream << "\"localizerX\":" << localizerPosition.x << ",";
  stream << "\"localizerY\":" << localizerPosition.y << ",";
  stream << "\"localizerOrientation\":" << localizerPosition.orientation << ",";
  stream << "\"odometerX\":" << odometerPosition.x << ",";
  stream << "\"odometerY\":" << odometerPosition.y << ",";
  stream << "\"odometerOrientation\":" << odometerPosition.orientation << ",";
  stream << "\"gotBall\":" << (dribbler->gotBall() ? "true" : "false") << ",";

  stream << "\"wheelFL\": {";
  stream << "\"stalled\":" << (wheelFL->isStalled() ? "true" : "false") << ",";
  stream << "\"targetOmega\":" << wheelFL->getTargetOmega() << ",";
  stream << "\"realOmega\":" << wheelFL->getRealOmega();
  stream << "},";

  stream << "\"wheelFR\": {";
  stream << "\"stalled\":" << (wheelFR->isStalled() ? "true" : "false") << ",";
  stream << "\"targetOmega\":" << wheelFR->getTargetOmega() << ",";
  stream << "\"realOmega\":" << wheelFR->getRealOmega();
  stream << "},";

  stream << "\"wheelRL\": {";
  stream << "\"stalled\":" << (wheelRL->isStalled() ? "true" : "false") << ",";
  stream << "\"targetOmega\":" << wheelRL->getTargetOmega() << ",";
  stream << "\"realOmega\":" << wheelRL->getRealOmega();
  stream << "},";

  stream << "\"wheelRR\": {";
  stream << "\"stalled\":" << (wheelRR->isStalled() ? "true" : "false") << ",";
  stream << "\"targetOmega\":" << wheelRR->getTargetOmega() << ",";
  stream << "\"realOmega\":" << wheelRR->getRealOmega();
  stream << "},";

  debugBallList("ballsRaw", stream, visibleBalls);
  debugBallList("ballsFiltered", stream, ballLocalizer->balls);

  stream << "\"cameraFOV\":" << currentCameraFOV.toJSON() << ",";

  stream << "\"tasks\": [";

  bool first = true;

  for (TaskQueueIt it = tasks.begin(); it != tasks.end(); it++) {
    Task *task = *it;

    if (!first)
      stream << ",";
    else
      first = false;

    stream << "{";
    stream << "\"started\": " << (task->isStarted() ? "true" : "false") << ",";
    stream << "\"percentage\": " << task->getPercentage() << ",";
    stream << "\"status\": \"" << task->toString() << "\"";
    stream << "}";
  }

  stream << "]";

  json = stream.str();

  frameTargetSpeedSet = false;
}