lcq.cc

该文件是包含了机器人足球比赛中的整个系统的代码

#include "LCQ.h"
#include <iostream>

using namespace std;

LCQ::LCQ()
{
//grabSuccess = true;
  isTheStrutIdle = true;
  isTailWagging = false;
  isEmergencyCancel = false;
  headCommand.Set(0.0,0.0,0.0,true);
  head.movingLeft_ = false;
  head.movingUp_ = false;
  head.moving_ = false;
  locomotionData.deltaForward = 0.0;
  locomotionData.deltaLeft = 0.0;
  locomotionData.deltaTurn = 0.0;

  previousLocomotionData.deltaForward = 0.0;
  previousLocomotionData.deltaLeft = 0.0;
  previousLocomotionData.deltaTurn = 0.0;

  memset(&currentLocomotionCommand, 0, sizeof(LocomotionCommand));

  numSteps = 0;
}

/*****************************************************************************/
/* Accessor Methods                                                          */
/*****************************************************************************/

bool LCQ::IsWalkOnly() {
  if (list.size() == 1) {
    if (Front().motionType == LocomotionCommand::TP_WALK || Front().motionType == LocomotionCommand::TP_WALKWITHOUTFRONT || Front().motionType == LocomotionCommand::TP_WALKTURNKICK/*|| Front().motionType == LocomotionCommand::TP_DONOTHING*/) {
      return true;
    }
  }
  return false;
}

int LCQ::Size()
{
  return list.size();
}

bool LCQ::IsEmpty()
{
  return list.empty();
}

LocomotionCommand LCQ::Front()
{
  return list.back().lc;
}

/*****************************************************************************/
/* Mutator Methods                                                           */
/*****************************************************************************/

int LCQ::Enqueue(int priority, LocomotionCommand lc)
{
  // When you add something of higher priority, clear the queue, and add it
  // ""                        equal priority and not 0, then just add it else equal priority and 0 then clear queue and then add
  // Otherwise don't add it and return -1

  // Create the node
//  cout << "LCQ - Enqueue " << endl << flush;

  Node n;
  n.priority = priority;
  n.lc = lc;

  // If the list is empty, then add it
  if (list.empty()) {
      list.push_back(n);
      return 0;
  }

  if (priority == -1) {
    list.insert(list.begin(), n);
    return list.size() - 1;
  }

// if this -1 thing is still on the queue, you can't add shit !
  if (list.front().priority == -1) {
      return 0;
  }

  // Get the highest priority from the front of the list
  int frontPriority = list.back().priority;

  // When you want to add something of higher priority, clear the queue then add it
  if (priority > frontPriority) {
    Clear();
    list.push_back(n);
    return list.size() - 1;
  }

  // Check if they are the same priority
  if (priority == frontPriority) {
    // If the priority is 0 then clear the list and add it
    if (priority == 0) {
      Clear();
      list.push_back(n);
      return list.size() - 1;
    } else {
      // Just add the node to the end of the list
      list.insert(list.begin(), n);
      return list.size() - 1;
    }
  }

  return -1;
}

LocomotionCommand LCQ::Dequeue()
{
  Node n = list.back();
  //cout << "LCQ: f" << n.lc.forward_ << "\ts:" << n.lc.strafe_ << "\tt:" << n.lc.turn_ << endl;
  list.pop_back();
  return n.lc;
}

void LCQ::Clear()
{
  list.clear();
}