12.5

Chassis/inc/leg.h

@@ -1,58 +1,58 @@
 //----
 // @file leg.h
 // @author mask <[email protected]>
-// @brief 
+// @brief
 // @version 1.0
 // @date 2023-11-12
-// 
+//
 // @copyright Copyright (c) 2023
-// 
+//
 //----
 
 #pragma once
 
-#include "tim.h"
-#include "tmotor.h"
-#include "djmotor.h"
 #include "datastruct.h"
+#include "djmotor.h"
 #include "robotparam.h"
+#include "tim.h"
+#include "tmotor.h"
 
 typedef struct {
-  char dir;
-  
-  DJmotor* wheel;
-  Tmotor* front;
-  Tmotor* behind;
-  Input X, Xd;
-  Output U;
-  datastruct angle0;
-  datastruct L0;
-  datastruct dis;
-  u32 timer;
-  // 五连杆坐标系下的坐标，原点在五连杆的中垂线上
-  float angle1, angle2, angle3, angle4; // 角度计算值和读取到的真实值，是和图中的一一对应
-  float angle1set, angle4set; // 角度设定值，是在初始角度之上的设定值
-  
-  float xa, ya;
-  float xb, yb;
-  float xc, yc;
-  float xd, yd;
-  float xe, ye;
-  float Fnow;
-  float Tpnow;
-  float TFnow;
-  float TBnow;
-  float TWheelnow;
-  float Fset; // 虚拟力
-  float Tpset; // 关节处扭矩
-  float TFset;
-  float TBset;
-  float TWheelset;
-  float normalforce; // 地面对机器人的实际支持力
-  float K[2][6];
-}Leg;
+	int dir;
+
+	DJmotor*	 wheel;
+	Tmotor*		 front;
+	Tmotor*		 behind;
+	Input			 X, Xd;
+	Output		 U;
+	datastruct angle0;
+	datastruct L0;
+	datastruct dis;
+	u32				 timer;
+	// 五连杆坐标系下的坐标，原点在五连杆的中垂线上
+	float			 angle1, angle2, angle3, angle4;	// 角度计算值和读取到的真实值，是和图中的一一对应
+	float			 angle1set, angle4set;						// 角度设定值，是在初始角度之上的设定值
+
+	float xa, ya;
+	float xb, yb;
+	float xc, yc;
+	float xd, yd;
+	float xe, ye;
+	float Fnow;
+	float Tpnow;
+	float TFnow;
+	float TBnow;
+	float TWheelnow;
+	float Fset;		// 虚拟力
+	float Tpset;	// 关节处扭矩
+	float TFset;
+	float TBset;
+	float TWheelset;
+	float normalforce;	// 地面对机器人的实际支持力
+	float K[2][6];
+} Leg;
 
-void legInit(Leg* leg, LegDir dir, DJmotor* wheel, Tmotor* front, Tmotor* behind);
+void legInit(Leg* leg, int dir, DJmotor* wheel, Tmotor* front, Tmotor* behind);
 void Zjie(Leg* leg, float pitch);
 void Njie(Leg* leg, float xc, float yc);
 void VMC(Leg* leg);

Chassis/inc/robot.h

@@ -1,37 +1,37 @@
 //----
 // @file robot.h
 // @author mask <[email protected]>
-// @brief 
+// @brief
 // @version 1.0
 // @date 2023-11-12
-// 
+//
 // @copyright Copyright (c) 2023
-// 
+//
 //----
 
 #pragma once
 
 #include "leg.h"
 #include "yesense.h"
 typedef struct {
-  bool flyflag;
-  
-  Leg legL;
-  Leg legR;
-  Leg legVir;
-  Yesense yesense;
-  
-  // 以下四个PID输出结果均为力
-  PID* yawpid; // 角速度控制
-  PID* rollpid; // 翻滚角控制
-  PID* splitpid; // 双腿劈叉控制
-  PID* L0pid; // 虚拟力的pid 是腿长的控制
-  
-  RobotRunMode mode;
-
-  float L0Set; // 设定腿长，也就是当前两条腿的平均腿长
-  float xSet;
-  float vSet;
+	bool flyflag;
+
+	Leg			legL;
+	Leg			legR;
+	Leg			legVir;
+	Yesense yesense;
+
+	// 以下四个PID输出结果均为力
+	PID* yawpid;		// 角速度控制
+	PID* rollpid;		// 翻滚角控制
+	PID* splitpid;	// 双腿劈叉控制
+	PID* L0pid;			// 虚拟力的pid 是腿长的控制
+
+	RobotRunMode mode;
+
+	float L0Set;	// 设定腿长，也就是当前两条腿的平均腿长
+	float xSet;
+	float vSet;
 } Robot;
 
 extern Robot* robot;

Chassis/inc/robotparam.h

@@ -4,67 +4,65 @@
 // @brief 机器人所需要的所有参数的定义，但是不完全，还需要测定
 // @version 1.0
 // @date 2023-11-13
-// 
+//
 // @copyright Copyright (c) 2023
-// 
+//
 //----
 #pragma once
 #include "vector.h"
 
-#define l1 0.18f
-#define l2 0.336f
-#define l3 0.336f
-#define l4 0.18f
-#define l5 0.24f
+// 初始状态：
+// angle4 -23.2
+// angle1 183.23
+
+#define l1		 0.18f
+#define l2		 0.336f
+#define l3		 0.336f
+#define l4		 0.18f
+#define l5		 0.24f
+#define WHEELR 0.65f
 
 // TODO: 重量测定和参数计算
-#define MASSL1 0.1f
-#define MASSL2 0.1f
-#define MASSL3 0.1f
-#define MASSL4 0.1f
-#define MASSBODY 0.1f
-#define HALFMASSBODY 0.1f
-#define MASSLEG 0.1f
-#define MASSWHEEL 0.1f
-#define GRAVITY 9.805f
+#define MASSL1					0.272f
+#define MASSL2					0.704f
+#define MASSL3					0.703f
+#define MASSL4					0.272f
+#define MASSBODY				4.764f
+#define HALFMASSBODY		2.382f
+#define MASSLEG					1.951f
+#define MASSWHEEL				0.71f
+#define GRAVITY					9.805f
 // 虚拟力前馈
-#define FFEEDFORWARD -0.1f
+#define FFEEDFORWARD		-23.35551f
 // 最小支持力阈值，判断是否离地的支持力的阈值
-#define FORCETHRESHOLD -20.f
-#define MAXROBOTSPEED 1.f
-#define MINROBOTLEGLEN 0.1f;
-#define MAXROBOTLEGLEN 0.3f;
+#define FORCETHRESHOLD	-20.f
+#define MAXROBOTSPEED		1.f
+#define MINROBOTLEGLEN	0.16f;
+#define MAXROBOTLEGLEN	0.4f;
 #define MAXROBOTLEGDIFF 0.1f;
-#define MAXROBOTROLL 1.f;
-#define MAXROBOTSPLIT 1;
-
-typedef enum {
-  LEGLEFT = (char) -1,
-  LEGRIGHT = (char) 1
-}LegDir;
+#define MAXROBOTROLL		1.f;
+#define MAXROBOTSPLIT		1;
+#define LEGLEFT					(int) 1
+#define LEGRIGHT				(int) -1
 
-typedef enum {
-  ROBOTNORMAL = 0,
-  ROBOTJUMP,
-  ROBOTHALT
-}RobotRunMode;
+typedef enum { ROBOTNORMAL = 0, ROBOTJUMP, ROBOTHALT } RobotRunMode;
 
 typedef struct {
-  float theta;
-  float thetadot;
-  float x;
-  float v;
-  float pitch;
-  float pitchdot;
-}Input;
+	float theta;
+	float thetadot;
+	float x;
+	float v;
+	float pitch;
+	float pitchdot;
+} Input;
 
 typedef struct {
-  float Tp;
-  float Twheel;
-}Output;
+	float Tp;
+	float Twheel;
+} Output;
 
 void inputInit(Input* input);
 void outputInit(Output* output);
 
 extern vector2f jumpPoints[4];
-extern float Kcoeff[12][4];
+extern float		Kcoeff[12][4];