lcq.h

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

#ifndef LCQ_H
#define LCQ_H

#include <vector>
#include <iostream>
#include "../Common/LocomotionCommand.h"
#include "../Common/HeadCommand.h"

using namespace std;

struct LocomotionData {
  double deltaForward;
  double deltaLeft;
  double deltaTurn;
};

class LCQ {
public:

  struct HeadData {
    bool movingLeft_;
    bool movingUp_;
    bool moving_;
  };

  HeadData head;

  struct Node {
    int priority;
    LocomotionCommand lc;
  };

  LCQ();

  int Size();
  bool IsEmpty();
  bool IsWalkOnly();
  LocomotionCommand Front();

  int Enqueue(int, LocomotionCommand);
  LocomotionCommand Dequeue();
  void Clear();

  void SetCurrentLocomotionCommand(LocomotionCommand lc) { currentLocomotionCommand = lc; }
  LocomotionCommand GetCurrentLocomotionCommand() { return currentLocomotionCommand; }

  void SetTheStrutIdle(bool tsi) { isTheStrutIdle = tsi; }
  bool IsTheStrutReady() { 
    if (isTheStrutIdle == false) return false;
    if (Size() == 0) return true;
    if (Size() == 1 && Front().motionType == LocomotionCommand::TP_DONOTHING) return true;
    return false;
// old way !
//  return (isTheStrutIdle && (Size()==0)); 
  }

  HeadCommand GetHeadCommand() { return headCommand; }
  void SetHeadCommand(HeadCommand hc) { headCommand = hc; }

  void SetLocomotionData(LocomotionData ld) {
    locomotionData = ld;
  }
  LocomotionData GetLocomotionData() {
    return locomotionData;
  }
  LocomotionData GetPreviousLocomotionData() {
    return previousLocomotionData;
  }
  // only call from thestrut!
  void AddLocomotionData(LocomotionData ld) {
    locomotionData.deltaForward+=ld.deltaForward;
    locomotionData.deltaLeft+=ld.deltaLeft;
    locomotionData.deltaTurn+=ld.deltaTurn;
  }
  // only call from locwm!
  void ClearLocomotionData() {
    previousLocomotionData.deltaForward = locomotionData.deltaForward;
    previousLocomotionData.deltaLeft = locomotionData.deltaLeft;
    previousLocomotionData.deltaTurn = locomotionData.deltaTurn;

    locomotionData.deltaForward = 0.0;
    locomotionData.deltaLeft = 0.0;
    locomotionData.deltaTurn = 0.0;
  }
/*
  // these can be called by both locwm and thestrut
  bool GetGrabSuccess() {
    return grabSuccess;
  }
  void SetGrabSuccess(bool gs) {
    grabSuccess = gs;
  }
*/

  bool IsEmergencyCancel() {
    bool iec = isEmergencyCancel;
    isEmergencyCancel=false;
    return iec;
  }
  void SetEmergencyCancel(bool iec) {
    isEmergencyCancel = iec;
  }
  bool IsTailWagging() {
    return isTailWagging;
  }
  void SetTailWagging(bool itw) {
    isTailWagging=itw;
  }

  void IncrementNumSteps() {
    numSteps++;
  }
  int GetNumSteps() {
    return numSteps;
  }
  void ResetNumSteps() { 
    numSteps = 0;
  }

private:
  vector<Node> list;

  LocomotionCommand currentLocomotionCommand;

  bool isTailWagging;
  bool isEmergencyCancel;
//bool grabSuccess;
  bool isTheStrutIdle;
  HeadCommand headCommand;
  LocomotionData locomotionData;
  LocomotionData previousLocomotionData; // so we can send to wireless !

  int numSteps;
};

#endif // LCQ_H