Cpp/JointControl.h at master · hyperchi/Cpp · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
#ifndef __JOINTCONTROL_H__
#define __JOINTCONTROL_H__

#include <iostream>
#include <stdio.h>
#include <vector>

#include "EnumType.h"
#include "WorldModel.h"
#include "CommandQuene.h"
#include "Logger.h"
#include "FloatMath.h"

using namespace std;

namespace joints
{
	struct Joint
	{
		float joint[HJCOUNT];
	};
};
using namespace joints;

class JointControl
{
public:
	JointControl(  WorldModel * wm, CommandQuene *cmdQuene );
	~JointControl();

private:
	WorldModel   *mWorldModel;
	CommandQuene *mCmdQuene;
	CommandPriority mCmdPri;
	vector<Joint> mJoints;

	bool mJointEnd;

public:
	unsigned int mDelayFrameNum;

	void ClearJoint();
	void ClearDelayJoint();
	void SetJointAngle( CommandPriority cmdPri );
	void SetJointAngle( CommandPriority cmdPri, Joint jointVec );
	void SetJointAngle( CommandPriority cmdPri, HJointT jointType, float jointAng );
	void SetHeadJointAngle( Angle yawAng, Angle pitchAng );
	void GetCommand();
	bool IsJointVectorEnd();

private:
	string GetJointCommand( const Joint &jointVec );


private:
	vector<float> mLastJointAngle;
	vector<Joint> mDelayJointAngle;
	//vector<Joint> mDelayWorldModelJointAngle;

	bool mUsePID;
	float mPIDEps;
	float mAngleMax;
	int	mErrorJointNum;

	void InitPID( );
	bool PID( Joint initJoint, Joint &tarJoint );
	bool PIDError( const float &diffAngle, const float &currAng, const float &preAng, float &tarAngle );
	bool PIDError( const float &angle, unsigned int num, float &aFact );

private:
	bool IsAgentOutOfControl();
	Joint ControlAgentToInitialPos();

private:
	bool mIsBeamAndInitial;
	float mBeamValue[3];

public:
	void SetBeamAndInitial( bool isBeamAndInitial);
	void SetBeamValue( float xBeam, float yBeam, float angBeam );
};

#endif //__JOINTCONTROL_H__