Cpp/BasicWalk.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
84
85
86
87
88
89
90
91
92
93
94
95
96
97
/*
按manual定义的坐标体系，P[a1,a2,a3]分别代表
a1:侧向
a2:前进方向
a3:垂直方向

*/
#ifndef __BASICWALK_H__
#define __BASICWALK_H__

#include <vector>
#include "Kinematics.h"
#include <math.h>
#include <cmath>
#include <string>
#include <iostream>
#include <fstream>
#include "JointControl.h"


#define LeftLegStay true
#define RightLegStay false

#define Cycle_Normal 10
#define Cycle_QuickTrans 4
#define Cycle_QuickTurn 4
class BasicWalk
{
public:
	BasicWalk();
	~BasicWalk();
	void BasicWalkInit();
    void CalWalk(float forLength, float transLengh, float turnAngle, vector<Joint> &tempjointsV);
	bool ResetFrameNum();
	bool SetCycle(int newCycle);
	void PrintDat();


private:
    void UpdateTarFoot(Kinematics *mLegS,Kinematics *mLegM,float xMove,float yMove,int i,float per,float NowPosS[],float  NowPosM[],float NowRS[],float NowRM[], vector<Joint> &tempjointsV);
	float CalNextMove(float t,float x,float v);

	void CalXY();
	void ChangeVtest();
	void RLegS(vector<Joint> &tempjointsV);
	void LLegS(vector<Joint> &tempjointsV);
	void Jumptest();
	void InverInit();//逆向运动学之前先将两腿初始的角度调整成一样，减少计算误差
	void StateInit();
	void InitCenterofM_X();
	void InitCenterofM_Y();

	void SetJoints(Joint &tempjoints);
	void Turnangle(Kinematics *tLeg,float NowR[],float tAngle);
	void KeepState(int i);

public:
	bool IsLeftLegStay;
	int FrameNum;
	int Cycle;

private:

	Kinematics* mLLeg;
	Kinematics* mRLeg;


	float mAngle[22];
	float x[20+1];
	float y[20+1];
	float detCenteOfMx;
	float detCenteOfMy_multi;
	float StepLenX;
	float StepLenY;
	float TurnAng; //radian
	float mZ;
	float Tc;
	float g;
	float T;
	float InitR[9];
	//float NowR[9];
	float NowR_L[9];
	float NowR_R[9];
	//InitP为初始状态下双脚与质心相对位置
	float InitP_L[3];
	float InitP_R[3];
	//NowP为一步之后(10帧)双脚与质心相对位置
	float NowP_L[3];
    float NowP_R[3];
	bool IsyMoveLeft;
	bool IsTurnLeft;
	bool Is2LegsTurn;
	//std::ofstream fop;


};
#endif