📄 thestrut.h

📁 该文件是包含了机器人足球比赛中的整个系统的代码
💻 H
📖 第 1 页 / 共 2 页
字号:
12 下一页
#ifndef _TheStrut_h_DEFINED
#define _TheStrut_h_DEFINED

#include <OPENR/OSubject.h>
#include <OPENR/OObserver.h> 

#include "../Common/Common.h"
#include "../Common/RobotState.h"
#include "../Common/LocomotionCommand.h"
#include "../Common/HeadCommand.h"
#include "Traction.h"

#include <fstream>
#include <iomanip>

#include <vector>
#include <sstream>
#include <string>

// blasphemy!
using namespace std;


// WARNING !!!! REPEAT OF DEFINITIONS IN Globals.h !!!
// (6 days till RoboCup. fuck it)
#define NUM_LEDS 6
#define NUM_LEDS_2 20


#define NUM_HEAD_JOINTS 3

#ifdef TAIL_ENABLED
  #define NUM_OTHER_JOINTS 3
#else
  #define NUM_OTHER_JOINTS 1
#endif

#define NUM_BODY_JOINTS 12

#define NUM_BUFFERS 2

#define NUM_LOCUS_POINTS 10
#define NUM_LOCUS_POINTS_INTERNAL 100


#ifdef ERS_7
static const char* const OTHER_JOINT[] = {
//  "PRM:/r1/c1/c2/c3/e5-Joint4:15", // Left ear
//  "PRM:/r1/c1/c2/c3/e6-Joint4:16", // Right ear
  "PRM:/r1/c1/c2/c3/c4-Joint2:14", // Mouth
#ifdef TAIL_ENABLED
  "PRM:/r6/c1-Joint2:61", // Tail tilt
  "PRM:/r6/c2-Joint2:62", // Tail pan
#endif
};

static const char* const BODY_JOINT[] = {
  "PRM:/r4/c1-Joint2:41",       // RFLEG J1 (Right Front Leg)
  "PRM:/r4/c1/c2-Joint2:42",    // RFLEG J2
  "PRM:/r4/c1/c2/c3-Joint2:43", // RFLEG J3


  "PRM:/r2/c1-Joint2:21",       // LFLEG J1 (Left Front Leg)
  "PRM:/r2/c1/c2-Joint2:22",    // LFLEG J2
  "PRM:/r2/c1/c2/c3-Joint2:23", // LFLEG J3

  "PRM:/r5/c1-Joint2:51",       // RRLEG J1 (Right Rear Leg)
  "PRM:/r5/c1/c2-Joint2:52",    // RRLEG J2
  "PRM:/r5/c1/c2/c3-Joint2:53", // RRLEG J3

  "PRM:/r3/c1-Joint2:31",       // LRLEG J1 (Left Rear Leg)
  "PRM:/r3/c1/c2-Joint2:32",    // LRLEG J2
  "PRM:/r3/c1/c2/c3-Joint2:33"  // LRLEG J3
};

static const char* const HEAD_JOINT[] =	{
  "PRM:/r1/c1-Joint2:11",	// TILT1 (Head Tilt1)
  "PRM:/r1/c1/c2-Joint2:12",	// PAN  (Head Pan)
  "PRM:/r1/c1/c2/c3-Joint2:13"  // TILT2 (Head Tilt2)
};

static const char* const LED_PRIMITIVES[] = {
  "PRM:/r1/c1/c2/c3/l1-LED2:l1", // Head light(color)
  "PRM:/r1/c1/c2/c3/l2-LED2:l2", // Head light(white)
  "PRM:/r1/c1/c2/c3/l3-LED2:l3", // Mode Indicator(red)
  "PRM:/r1/c1/c2/c3/l4-LED2:l4", // Mode Indicator(green)
  "PRM:/r1/c1/c2/c3/l5-LED2:l5", // Mode Indicator(blue)
  "PRM:/r1/c1/c2/c3/l6-LED2:l6", // Wireless light
  "PRM:/r1/c1/c2/c3/la-LED3:la", // Face light1
  "PRM:/r1/c1/c2/c3/lb-LED3:lb", // Face light2
  "PRM:/r1/c1/c2/c3/lc-LED3:lc", // Face light3
  "PRM:/r1/c1/c2/c3/ld-LED3:ld", // Face light4
  "PRM:/r1/c1/c2/c3/le-LED3:le", // Face light5
  "PRM:/r1/c1/c2/c3/lf-LED3:lf", // Face light6
  "PRM:/r1/c1/c2/c3/lg-LED3:lg", // Face light7
  "PRM:/r1/c1/c2/c3/lh-LED3:lh", // Face light8
  "PRM:/r1/c1/c2/c3/li-LED3:li", // Face light9
  "PRM:/r1/c1/c2/c3/lj-LED3:lj", // Face light10
  "PRM:/r1/c1/c2/c3/lk-LED3:lk", // Face light11
  "PRM:/r1/c1/c2/c3/ll-LED3:ll", // Face light12
  "PRM:/r1/c1/c2/c3/lm-LED3:lm", // Face light13
  "PRM:/r1/c1/c2/c3/ln-LED3:ln", // Face light14

  "PRM:/lu-LED3:lu", // Back light(front, color)
  "PRM:/lv-LED3:lv", // Back light(front, white)
  "PRM:/lw-LED3:lw", // Back light(middle, color)
  "PRM:/lx-LED3:lx", // Back light(middle, white)
  "PRM:/ly-LED3:ly", // Back light(rear, color)
  "PRM:/lz-LED3:lz", // Back light(rear, white)
};
#endif

#ifdef ERS_210
static const char* const OTHER_JOINT[] = {
//  "PRM:/r1/c1/c2/c3/e1-Joint3:j5", // ear1
//  "PRM:/r1/c1/c2/c3/e2-Joint3:j6", // ear2
  "PRM:/r1/c1/c2/c3/c4-Joint2:j4", // mouth
  "PRM:/r6/c1-Joint2:j1", // tail 1
  "PRM:/r6/c2-Joint2:j2"  // tail 2
};


static const char* const BODY_JOINT[] = {
  "PRM:/r4/c1-Joint2:j1",       // RFLEG J1 (Right Front Leg)
  "PRM:/r4/c1/c2-Joint2:j2",    // RFLEG J2
  "PRM:/r4/c1/c2/c3-Joint2:j3", // RFLEG J3

  "PRM:/r2/c1-Joint2:j1",       // LFLEG J1 (Left Front Leg)
  "PRM:/r2/c1/c2-Joint2:j2",    // LFLEG J2
  "PRM:/r2/c1/c2/c3-Joint2:j3", // LFLEG J3

  "PRM:/r5/c1-Joint2:j1",       // RRLEG J1 (Right Rear Leg)
  "PRM:/r5/c1/c2-Joint2:j2",    // RRLEG J2
  "PRM:/r5/c1/c2/c3-Joint2:j3", // RRLEG J3

  "PRM:/r3/c1-Joint2:j1",       // LRLEG J1 (Left Rear Leg)
  "PRM:/r3/c1/c2-Joint2:j2",    // LRLEG J2
  "PRM:/r3/c1/c2/c3-Joint2:j3"  // LRLEG J3
};

static const char* const HEAD_JOINT[] =	{
  "PRM:/r1/c1-Joint2:j1",	// TILT (Head Tilt)
  "PRM:/r1/c1/c2-Joint2:j2",	// PAN  (Head Pan)
  "PRM:/r1/c1/c2/c3-Joint2:j3"  // ROLL (Head Roll)
};

static const char* const LED_PRIMITIVES[] = {
  "PRM:/r1/c1/c2/c3/l1-LED2:l1", // Head LED 1
  "PRM:/r1/c1/c2/c3/l2-LED2:l2", // 2
  "PRM:/r1/c1/c2/c3/l3-LED2:l3", // 3
  "PRM:/r1/c1/c2/c3/l4-LED2:l4", // 4
  "PRM:/r1/c1/c2/c3/l5-LED2:l5", // 5
  "PRM:/r1/c1/c2/c3/l6-LED2:l6", // 6
  "PRM:/r1/c1/c2/c3/l7-LED2:l7", // 7
  "PRM:/r6/l1-LED2:l1", // Tail LED 1
  "PRM:/r6/l2-LED2:l2"  // 2
};

const int HEAD_LED_FR	= 0;		
const int HEAD_LED_MR	= 1;
const int HEAD_LED_BR	= 2;
const int HEAD_LED_FL	= 3;
const int HEAD_LED_ML	= 4;
const int HEAD_LED_BL = 5;
const int HEAD_LED_TOP = 6;
const int TAIL_LED_BLUE = 7;
const int TAIL_LED_RED = 8;

#endif

#ifdef ERS_7
  const int HEAD_LED_COLOR = 0;
  const int HEAD_LED_WHITE = 1;
  const int MODE_INDICATOR_RED = 2;
  const int MODE_INDICATOR_GREEN = 3;
  const int MODE_INDICATOR_BLUE = 4;
  const int WIRELESS_LIGHT = 5;
  const int FACE_LIGHT1 = 0;
  const int FACE_LIGHT2 = 1;
  const int FACE_LIGHT3 = 2;
  const int FACE_LIGHT4 = 3;
  const int FACE_LIGHT5 = 4;
  const int FACE_LIGHT6 = 5;
  const int FACE_LIGHT7 = 6;
  const int FACE_LIGHT8 = 7;
  const int FACE_LIGHT9 = 8;
  const int FACE_LIGHT10 = 9;
  const int FACE_LIGHT11 = 10;
  const int FACE_LIGHT12 = 11;
  const int FACE_LIGHT13 = 12;
  const int FACE_LIGHT14 = 13;
  const int BACK_FRONT_LIGHT_COLOR = 14;
  const int BACK_FRONT_LIGHT_WHITE = 15;
  const int BACK_MIDDLE_LIGHT_COLOR = 16;
  const int BACK_MIDDLE_LIGHT_WHITE = 17;
  const int BACK_REAR_LIGHT_COLOR = 18;
  const int BACK_REAR_LIGHT_WHITE = 19;

#endif

class TheStrut {
  public:
    TheStrut();
    void Init(OSubject* scv) { sbjCommandVector = scv; }

    void IncJointsReady();
    void IncRobotState();

    void LoadParameters(char* parameterFile, char* odometryFile);
    void SetupBuffers();
    void OpenPrimitives();
    void InitialiseJoints();
    void SetRobotLeds();

    void SetJointGainManual();

    OSubject* sbjCommandVector;

    void SetJointGain();

//  void ReadKicks(char* filename);
    void XMLGetNextTag(char* filebuffer, int* index, int length, char* nextTag);
    void XMLVerifyNextTag(char* filebuffer, int* index, int length, char* nextTag);
    void XMLGetStringEntry(char* filebuffer, int* index, int length, char* stringEntry);
    void XMLGetIntEntry(char* filebuffer, int* index, int length, int* ret);
    void XMLGetDoubleEntry(char* filebuffer, int* index, int length, double* ret);
    void XMLParseKicksFile(const char* filename);



    bool GetNextLocomotionCommand();
    void GetNextHeadCommand();
    void InterpretLocomotionCommand(LocomotionCommand* lc, bool enableInterpolation = true);

    void DoLocusScaling(double** origLocusPoints, double sl, double sh, double** finalLocusPoints, double* topPerim, double *botPerim);
    void LocusInterpolation(double** origLocusPoints, double topPerim, double botPerim, double** finalLocusPoints, int* nP);

    void CalculateComponents_TZ(double *f, double *s, double *h, double timeParameter, double fMax, double sMax, double bSMax, double sh, double lfo, double lso, double lh);
    void CalculateComponents_EL(double *f, double *s, double *h, double timeParameter, double fMax, double sMax, double bSMax, double sh, double lfo, double lso, double lh);
    void CalculateComponents_R(double *f, double *s, double *h, double timeParameter, double fMax, double sMax, double bSMax, double sh, double lfo, double lso, double lh);
    void CalculateComponents_A(double *f, double *s, double *h, double timeParameter, int numPoints, double** locusPoints, double sMax, double bSMax, double sh, double lfo, double lso, double lh);

    void CalculateAngles_F(double f, double s, double h, double *ang1, double *ang2, double *ang3);
    void CalculateAngles_B(double f, double s, double h, double *ang1, double *ang2, double *ang3);

    void DoWalk();
    void DoKick();
    void DoInterpolate();
    void DoShakeYourBooty();
    void DoDefaultStance();
    void DoHeadLocomotion();
    void DoNothing();

    void CheckIfFallen();

    bool ShouldInterpolate(bool includeHead);
    void FinishInterpolation();

    int FindFreeBodyRegionID();
    int FindFreeHeadRegionID();
    int FindFreeOtherRegionID();
    int FindFreeLedRegionID();

    // dest, value, joint number
    void SetBodyJoint(long*, long, int);
    void SetHeadJoint(long*, long, int);

    int FR_numPoints;
    int FL_numPoints;
    int BR_numPoints;
    int BL_numPoints;

    // scaled and interpolated arbitrary locus points
    double** FR_locusPoints;
    double** FL_locusPoints;
    double** BR_locusPoints;
    double** BL_locusPoints;
12 下一页
⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -