locomotioncommand.cc

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

#include "LocomotionCommand.h"

double LocomotionCommand::defaultFrontStrideHeight;
double LocomotionCommand::defaultFrontForwardOffset;
double LocomotionCommand::defaultFrontSideOffset;
double LocomotionCommand::defaultFrontHeight;

double LocomotionCommand::defaultBackStrideHeight;
double LocomotionCommand::defaultBackForwardOffset;
double LocomotionCommand::defaultBackSideOffset;
double LocomotionCommand::defaultBackHeight;

int LocomotionCommand::defaultLocusType;

void LocomotionCommand::SetDefaults(double _frontHeight, double _frontStrideHeight, double _frontForwardOffset, double _frontSideOffset, double _backHeight, double _backStrideHeight, double _backForwardOffset, double _backSideOffset, int _locusType) {
  defaultFrontStrideHeight = _frontStrideHeight;
  defaultFrontForwardOffset = _frontForwardOffset;
  defaultFrontSideOffset = _frontSideOffset;
  defaultFrontHeight = _frontHeight;

  defaultBackStrideHeight = _backStrideHeight;
  defaultBackForwardOffset = _backForwardOffset;
  defaultBackSideOffset = _backSideOffset;
  defaultBackHeight = _backHeight;

  defaultLocusType = _locusType;
}


void LocomotionCommand::Set(int _type, double  _frontStrideLength, double _backStrideLength, double _turn, double _strafe, double _stepFrequency, double _frontHeight, double _frontStrideHeight, double _frontForwardOffset, double _frontSideOffset, double _backHeight, double _backStrideHeight, double _backForwardOffset, double _backSideOffset) {
  motionType = _type;
  stepFrequency = _stepFrequency;
  frontStrideLength = _frontStrideLength;
  backStrideLength = _backStrideLength;
   
  turn = _turn;
  strafe = _strafe;

  odomTurn = turn;

  frontHeight = _frontHeight;
  frontStrideHeight = _frontStrideHeight;
  frontForwardOffset = _frontForwardOffset;
  frontSideOffset = _frontSideOffset;

  backHeight = _backHeight;
  backStrideHeight = _backStrideHeight;
  backForwardOffset = _backForwardOffset;
  backSideOffset = _backSideOffset;

  locusType = defaultLocusType;
  offsetWalk = false;
}

void LocomotionCommand::Set(int _type, double _frontStrideLength, double _backStrideLength, double _turn, double _strafe, double _stepFrequency) {
  motionType = _type;
  stepFrequency = _stepFrequency;
  frontStrideLength = _frontStrideLength;
  backStrideLength = _backStrideLength;

  turn = _turn;
  strafe = _strafe;

  odomTurn = turn;

  frontHeight = defaultFrontHeight;
  frontStrideHeight = defaultFrontStrideHeight;
  frontForwardOffset = defaultFrontForwardOffset;
  frontSideOffset = defaultFrontSideOffset;

  backHeight = defaultBackHeight;
  backStrideHeight = defaultBackStrideHeight;
  backForwardOffset = defaultBackForwardOffset;
  backSideOffset = defaultBackSideOffset;

  locusType = defaultLocusType;
  offsetWalk = false;
}

void LocomotionCommand::SetKick(int _type, bool _allowHeadUse) {
  motionType = _type;
  allowHeadUse = _allowHeadUse;

  stepFrequency = 0.0;
  frontStrideLength = 0.0;
  backStrideLength = 0.0;

  turn = 0.0;
  strafe = 0.0;

  odomTurn = turn;

  frontHeight = defaultFrontHeight;
  frontStrideHeight = defaultFrontStrideHeight;
  frontForwardOffset = defaultFrontForwardOffset;
  frontSideOffset = defaultFrontSideOffset;

  backHeight = defaultBackHeight;
  backStrideHeight = defaultBackStrideHeight;
  backForwardOffset = defaultBackForwardOffset;
  backSideOffset = defaultBackSideOffset;

  locusType = defaultLocusType;
  offsetWalk = false;
}

void LocomotionCommand::CombineLocomotionCommands(LocomotionCommand* first, LocomotionCommand* second, double x) {
  //if (x < 0.5) {
    motionType = first->motionType;
    allowHeadUse = first->allowHeadUse;
    locusType = first->locusType;
  //} else {
  //  motionType = second->motionType;
  //  allowHeadUse = second->allowHeadUse;
  //  locusTtype = second->locusType;
 // }

  stepFrequency = (second->stepFrequency - first->stepFrequency)*x + first->stepFrequency;
  frontStrideLength = (second->frontStrideLength - first->frontStrideLength)*x + first->frontStrideLength;
  backStrideLength = (second->backStrideLength - first->backStrideLength)*x + first->backStrideLength;

  turn = (second->turn - first->turn)*x + first->turn;
  strafe = (second->strafe - first->strafe)*x + first->strafe;

  odomTurn = turn;

  frontHeight = (second->frontHeight - first->frontHeight)*x + first->frontHeight;
  frontStrideHeight = (second->frontStrideHeight - first->frontStrideHeight)*x + first->frontStrideHeight;
  frontForwardOffset = (second->frontForwardOffset - first->frontForwardOffset)*x + first->frontForwardOffset;
  frontSideOffset = (second->frontSideOffset - first->frontSideOffset)*x + first->frontSideOffset;
  backHeight = (second->backHeight - first->backHeight)*x + first->backHeight; 
  backStrideHeight = (second->backStrideHeight - first->backStrideHeight)*x + first->backStrideHeight;
  backForwardOffset = (second->backForwardOffset - first->backForwardOffset)*x + first->backForwardOffset;
  backSideOffset = (second->backSideOffset - first->backSideOffset)*x + first->backSideOffset;

  offsetWalk = false;
}