Skip to content

Commit

Permalink
5.5
Browse files Browse the repository at this point in the history
  • Loading branch information
@Luo25177
Luo25177 committed May 5, 2024
1 parent 670dff5 commit d5c3660
Show file tree
Hide file tree
Showing 31 changed files with 761 additions and 763 deletions.
38 changes: 19 additions & 19 deletions Chassis/inc/leg.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,20 +21,20 @@
typedef struct {
int dir;

Zdrive* wheel;
Tmotor* front;
Tmotor* behind;
Input X, Xd;
Output U;
Zdrive *wheel;
Tmotor *front;
Tmotor *behind;
Input X, Xd;
Output U;
DataStruct theta;
DataStruct L0;
DataStruct dis;
vu32 timer;
vu32 timer;
// 五连杆坐标系下的坐标，原点在五连杆的中垂线上
float angle0, angle1, angle2, angle4; // 角度计算值和读取到的真实值，是和图中的一一对应
float angle1set, angle4set; // 角度设定值，是在初始角度之上的设定值
float angle0, angle1, angle2, angle4;// 角度计算值和读取到的真实值，是和图中的一一对应
float angle1set, angle4set; // 角度设定值，是在初始角度之上的设定值
DataStruct angle3;

float xa, ya;
float xb, yb;
float xc, yc;
Expand All @@ -45,19 +45,19 @@ typedef struct {
float TFnow;
float TBnow;
float TWheelnow;
float Fset; // 虚拟力
float Tpset; // 关节处扭矩
float Fset; // 虚拟力
float Tpset;// 关节处扭矩
float TFset;
float TBset;
float TWheelset;
float normalforce; // 地面对机器人的实际支持力
float normalforce;// 地面对机器人的实际支持力
float K[2][6];
PID L0pid; // 虚拟力的pid 是腿长的控制
PID L0pid;// 虚拟力的pid 是腿长的控制
} Leg;

void LegInit(Leg* leg, int dir, Zdrive* wheel, Tmotor* front, Tmotor* behind);
void LegUpdate(Leg* leg);
void Zjie(Leg* leg, float pitch);
void Njie(Leg* leg, float xc, float yc);
void VMC(Leg* leg);
void INVMC(Leg* leg);
void LegInit(Leg *leg, int dir, Zdrive *wheel, Tmotor *front, Tmotor *behind);
void LegUpdate(Leg *leg);
void Zjie(Leg *leg, float pitch);
void Njie(Leg *leg, float xc, float yc);
void VMC(Leg *leg);
void INVMC(Leg *leg);
10 changes: 5 additions & 5 deletions Chassis/inc/linetraj.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
#include <math.h>

typedef struct {
bool finish;
bool finish;
float xset;
float speed;
float x;
Expand All @@ -16,7 +16,7 @@ typedef struct {
float speedDownDis;
} LineTraj;

void LineTrajSpeedUp(LineTraj* linetraj);
void LineTrajSpeedDown(LineTraj* linetraj);
void LineTrajRun(LineTraj* linetraj, float xnow);
void LineTrajInit(LineTraj* linetraj, float startspeed, float uniformspeed, float endspeed, float aimx, float speedUpDis, float speedDownDis);
void LineTrajSpeedUp(LineTraj *linetraj);
void LineTrajSpeedDown(LineTraj *linetraj);
void LineTrajRun(LineTraj *linetraj, float xnow);
void LineTrajInit(LineTraj *linetraj, float startspeed, float uniformspeed, float endspeed, float aimx, float speedUpDis, float speedDownDis);
16 changes: 8 additions & 8 deletions Chassis/inc/robot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,21 +20,21 @@

typedef struct {
bool flyflag;
u8 jumpPhase;
u8 jumpPhase;

Leg legL;
Leg legR;
Leg legVir;
Leg legL;
Leg legR;
Leg legVir;
Yesense yesense;

// 以下四个PID输出结果均为力
PID yawpid; // 角速度控制
PID rollpid; // 翻滚角控制
PID splitpid; // 双腿劈叉控制
PID yawpid; // 角速度控制
PID rollpid; // 翻滚角控制
PID splitpid;// 双腿劈叉控制

RobotRunMode mode;

float L0Set; // 设定腿长，也就是当前两条腿的平均腿长
float L0Set;// 设定腿长，也就是当前两条腿的平均腿长
float xSet;
float vSet;
float force;
Expand Down
54 changes: 27 additions & 27 deletions Chassis/inc/robotparam.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,45 +18,45 @@
// #define FrontAngleInit 1.762782545f
// #define BehindAngleInit 1.378810109f

#define FrontAngleInit 3.4033920414f
#define FrontAngleInit 3.4033920414f
#define BehindAngleInit -0.2617993878f
// 杆长
#define l1 0.15f
#define l2 0.27f
#define l3 0.27f
#define l4 0.15f
#define l5 0.15f
#define WHEELR 0.1f
#define l1 0.15f
#define l2 0.27f
#define l3 0.27f
#define l4 0.15f
#define l5 0.15f
#define WHEELR 0.1f
// 质量参数
#define MASSL1 0.2512f
#define MASSL2 0.3006f
#define MASSL3 0.33795f
#define MASSL4 0.22125f
#define MASSBODY 5.4940204f
#define HALFMASSBODY 2.7470102f
#define MASSLEG 1.111f
#define MASSWHEEL 0.71f
#define MASSL1 0.2512f
#define MASSL2 0.3006f
#define MASSL3 0.33795f
#define MASSL4 0.22125f
#define MASSBODY 5.4940204f
#define HALFMASSBODY 2.7470102f
#define MASSLEG 1.111f
#define MASSWHEEL 0.71f
// 重力
#define GRAVITY 9.805f
#define GRAVITY 9.805f
// 虚拟力前馈
#define FFEEDFORWARD 26.934435011f
#define FFEEDFORWARD 35.f
// 最小支持力阈值，判断是否离地的支持力的阈值
#define FORCETHRESHOLD 5.f
#define FORCETHRESHOLD 5.f
// 机器人最大速度，如果追踪更大的速度会翻车
#define MAXROBOTSPEED 1.f
#define MAXROBOTSPEED 1.f
// 最小腿长
#define MINROBOTLEGLEN 0.12f;
#define MINROBOTLEGLEN 0.12f;
// 最大腿长
#define MAXROBOTLEGLEN 0.40f;
#define MAXROBOTLEGLEN 0.40f;
// 最大腿部长度差
#define MAXROBOTLEGDIFF 0.1f;
// 最大翻滚角
#define MAXROBOTROLL 1.f;
#define MAXROBOTROLL 1.f;
// 最大腿部劈开的角度
#define MAXROBOTSPLIT 1;
#define MAXROBOTSPLIT 1;
// 腿部方向值 由于电机转动方向是相同的
#define LEGLEFT (int) -1
#define LEGRIGHT (int) 1
#define LEGLEFT (int) -1
#define LEGRIGHT (int) 1

typedef enum { ROBOTNORMAL,
ROBOTHALT } RobotRunMode;
Expand Down Expand Up @@ -95,8 +95,8 @@ typedef struct {
typedef struct {
} WBCOutput;

void InputInit(Input* input);
void OutputInit(Output* output);
void InputInit(Input *input);
void OutputInit(Output *output);

extern float Kcoeff[12][4];
extern float Kcoeff_wbc[40][6];
Expand Down
Loading

0 comments on commit d5c3660

Please sign in to comment.