11.16

Chassis/inc/leg.h

@@ -11,12 +11,57 @@
 
 #pragma once
 
-#include "djmotor.h"
+#include "tim.h"
 #include "tmotor.h"
-
-
+#include "djmotor.h"
+#include "datastruct.h"
+#include "robotparam.h"
 
 typedef struct {
   DJmotor* wheel;
   Tmotor* front, behind;
+  float angle1, angle2, angle3, angle4; // 角度计算值和读取到的真实值，是和图中的一一对应
+  float angle1set, angle4set; // 角度设定值，是在初始角度之上的设定值
+  datastruct angle0;
+  datastruct L0;
+  datastruct dis;
+
+  char dir;
+
+  unsigned int timer;
+
+  // 五连杆坐标系下的坐标，原点在五连杆的中垂线上
+  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;
+
+  PID Fpid; // 虚拟力的pid
+  float normalforce; // 地面对机器人的实际支持力
+  bool flyflag;
+
+  Input X, Xd;
+  Output U;
+
+  float K[2][6];
 }Leg;
+
+void legInit(Leg* leg, LegDir dir);
+void Zjie(Leg* leg, float pitch);
+void Njie(Leg* leg, float xc, float yc);
+void VMC(Leg* leg);
+void INVMC(Leg* leg);
+void flyCheck(Leg* leg, float accely);

Chassis/inc/robot.h

@@ -10,7 +10,31 @@
 //----
 
 #pragma once
-
+#include "leg.h"
+#include "yesense.h"
 typedef struct {
+  Leg legL;
+  Leg legR;
+  Leg legSim;
+  Yesense yesense;
+  // 角速度控制
+  PID yawpid;
+  // 翻滚角控制
+  PID rollpid;
+  // 双腿劈叉控制
+  PID splitpid;
+
+  u8 mode;
+} Robot;
+
+extern Robot* robot;
+
+void robotInit();
+
+void updateState();
+
+void balanceMode();
+
+void jumpMode();
 
-} Robot;
+void flyMode();

Chassis/inc/robotparam.h

@@ -1,32 +1,48 @@
 //----
 // @file robotparam.h
 // @author mask <[email protected]>
-// @brief 
+// @brief 机器人所需要的所有参数的定义，但是不完全需要测定
 // @version 1.0
 // @date 2023-11-13
 // 
 // @copyright Copyright (c) 2023
-// @details 机器人所需要的所有参数的定义，但是不完全需要测定
+// 
 //----
 #pragma once
 
-#define L1 0.18f
-#define L2 0.336f
-#define L3 0.336f
-#define L4 0.18f
-#define L5 0.24f
+#define l1 0.18f
+#define l2 0.336f
+#define l3 0.336f
+#define l4 0.18f
+#define l5 0.24f
 
 // TODO: 重量测定和参数计算
-#define MASSL1 
-#define MASSL2 
-#define MASSL3 
-#define MASSL4 
-#define MASSWHEEL
-#define MASSBODY
+#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
 
 float Kcoeff[12][4] = {0};
 
+typedef enum {
+  LEFT = (char) -1,
+  RIGHT = (char) 1
+}LegDir;
+
+typedef enum {
+  NORMAL,
+  JUMP,
+  SOAR
+}RobotRunMode;
+
+
 typedef struct {
   float theta;
   float thetadot;
@@ -39,4 +55,7 @@ typedef struct {
 typedef struct {
   float Tp;
   float Twheel;
-}Output;
+}Output;
+
+void inputInit(Input* input);
+void outputInit(Output* output);

Chassis/src/leg.c

@@ -0,0 +1,131 @@
+#include "leg.h"
+
+void legInit(Leg* leg, LegDir dir) {
+  leg->Fset = -(HALFMASSBODY + MASSLEG) * GRAVITY; // 虚拟力设定值的初始化
+  leg->dir = dir;
+
+  datastructInit(&leg->dis, 0, 0, 0, 0);
+  // TODO: L0 初始位置 angle0的初始值
+  datastructInit(&leg->L0, 0, 0, 0, 0);
+  datastructInit(&leg->angle0, 0, 0, 0, 0);
+
+  leg->Fnow = leg->Fset;
+  leg->Tpset = 0;
+  leg->TFset = 0;
+  leg->TBset = 0;
+  leg->TWheelset = 0;
+  leg->normalforce = leg->Fset;
+
+  inputInit(&leg->X);
+  inputInit(&leg->Xd);
+  outputInit(&leg->U);
+}
+
+void Zjie(Leg* leg, float pitch) {
+  leg->angle1 = leg->front->real.angle;
+  leg->angle4 = leg->behind->real.angle;
+
+  leg->xb = l1 * cos(leg->angle1) - l5 / 2;
+  leg->yb = l1 * sin(leg->angle1);
+  leg->xd = l5 / 2 + l4 * cos(leg->angle4);
+  leg->yd = l4 * sin(leg->angle4);
+  float lbd = sqrt(pow((leg->xd - leg->xb), 2) + pow((leg->yd - leg->yb), 2));
+  float A0 = 2 * l2 * (leg->xd - leg->xb);
+  float B0 = 2 * l2 * (leg->yd - leg->yb);
+  float C0 = pow(l2, 2) + pow(lbd, 2) - pow(l3, 2);
+  float D0 = pow(l3, 2) + pow(lbd, 2) - pow(l2, 2);
+  leg->angle2 = 2 * atan((B0 + sqrt(pow(A0, 2) + pow(B0, 2) - pow(C0, 2))) / (A0 + C0));
+  leg->angle3 = PI - 2 * atan((B0 + sqrt(pow(A0, 2) + pow(B0, 2) - pow(D0, 2))) / (A0 + D0));
+  leg->xc = leg->xb + l2 * cos(leg->angle2);
+  leg->yc = leg->yb + l2 * sin(leg->angle2);
+
+  float dt = (float) (GolbalTimer - leg->timer) / 1000;
+  leg->L0.now = sqrt(pow(leg->xc, 2) + pow(leg->yc, 2));
+
+  // 乘以pitch的旋转矩阵
+  float matrix_R[2][2] = {cos(pitch), -sin(pitch), sin(pitch), cos(pitch)};
+  float cor_XY[2][1] = {leg->xc, leg->yc};
+  float cor_XY_then[2][1];
+  cor_XY_then[0][0] = matrix_R[0][0] * cor_XY[0][0] + matrix_R[0][1] * cor_XY[1][0];
+  cor_XY_then[1][0] = matrix_R[1][0] * cor_XY[0][0] + matrix_R[1][1] * cor_XY[1][0];
+  leg->angle0.last = leg->angle0.now;
+  leg->angle0.now = atan(cor_XY_then[0][0] / cor_XY_then[1][0]);
+  if(dt > 0) {
+    leg->L0.dot = (leg->L0.now - leg->L0.last) / dt;
+    leg->L0.ddot = (leg->L0.dot - leg->L0.lastdot) / dt;
+    leg->L0.last = leg->L0.now;
+    leg->L0.lastdot = leg->L0.dot;
+    leg->angle0.dot = (leg->angle0.now - leg->angle0.last) / dt;
+    leg->angle0.ddot = (leg->angle0.dot - leg->angle0.lastdot) / dt;
+    leg->angle0.last = leg->angle0.now;
+    leg->angle0.lastdot = leg->angle0.dot;
+  }
+}
+
+void Njie(Leg* leg, float xc, float yc) {
+  float m, n, b, x1, y1;
+  float A, B, C;
+
+  A = 2 * l1 * yc;
+  B = 2 * l1 * (xc + l5 / 2);
+  C = l2 * l2 - l1 * l1 - xc * xc - yc * yc - l5 * l5 / 4 + xc * l5;
+  leg->angle1set = 2 * atan((A + sqrt(A * A + B * B - C * C)) / (B - C));
+  if (leg->angle1set < 0)
+    leg->angle1set += 2 * PI;
+
+  // nije_5(&angle1, (void *)0, x, y, l1, l6, l3, l4, l5); //利用L1,L6计算c1;
+  m = l1 * cos(leg->angle1set);
+  n = l1 * sin(leg->angle1set);
+  b = 0;
+  // x1 = l2 / l6 * ((x - m) * cos(b) - (y - n) * sin(b)) + m;
+  // y1 = l2 / l6 * ((x - m) * sin(b) + (y - n) * cos(b)) + n; //得到闭链五杆端点的坐标
+  x1 = ((xc - m) * cos(b) - (yc - n) * sin(b)) + m;
+  y1 = ((xc - m) * sin(b) + (yc - n) * cos(b)) + n; // 得到闭链五杆端点的坐标
+
+  A = 2 * y1 * l4;
+  B = 2 * l4 * (x1 - l5 / 2);
+  // c = l3 * l3 + 2 * l5 * x1 - l4 * l4 - l5 * l5 - x1 * x1 - y1 * y1;
+  C = l3 * l3 + l5 * x1 - l4 * l4 - l5 * l5 / 4 - x1 * x1 - y1 * y1;
+  leg->angle4set = 2 * atan((A - sqrt(A * A + B * B - C * C)) / (B - C));
+
+  // TODO: 根据初始角度来纠正设定角度
+  // leg->angle1set = PI - leg->angle1set - PI / 3;
+  // leg->angle4set -= PI / 3;
+  // nije_5((void *)0, &angle2, x1, y1, l1, l2, l3, l4, l5);        //计算c4 ,
+}
+
+void VMC(Leg* leg) {
+  float trans[2][2] = {l1 * cos(leg->angle0.now + leg->angle3) * sin(leg->angle1 - leg->angle2) / sin(leg->angle2 - leg->angle3),
+                       l1 * sin(leg->angle0.now + leg->angle3) * sin(leg->angle1 - leg->angle2) / (leg->L0.now * sin(leg->angle2 - leg->angle3)),
+                       l4 * cos(leg->angle0.now + leg->angle2) * sin(leg->angle3 - leg->angle4) / sin(leg->angle2 - leg->angle3),
+                       l4 * sin(leg->angle0.now + leg->angle2) * sin(leg->angle3 - leg->angle4) / (leg->L0.now * sin(leg->angle2 - leg->angle3))};
+  float virtualF[2][1] = {leg->Fset, leg->Tpset};
+  leg->TFset = trans[0][0] * virtualF[0][0] + trans[0][1] * virtualF[1][0];
+  leg->TBset = trans[1][0] * virtualF[0][0] + trans[1][1] * virtualF[1][0];
+}
+
+// TODO: 没测试过不一定没问题
+void INVMC(Leg* leg) {
+  float trans[2][2] = {-(sin(leg->angle0.now + leg->angle2)*sin(leg->angle2 - leg->angle3))/(l1*cos(leg->angle0.now + leg->angle2)*sin(leg->angle0.now + leg->angle3)*sin(leg->angle1 - leg->angle2) - l1*cos(leg->angle0.now + leg->angle3)*sin(leg->angle0.now + leg->angle2)*sin(leg->angle1 - leg->angle2)), 
+                      (sin(leg->angle0.now + leg->angle3)*sin(leg->angle2 - leg->angle3))/(l4*cos(leg->angle0.now + leg->angle2)*sin(leg->angle0.now + leg->angle3)*sin(leg->angle3 - leg->angle4) - l4*cos(leg->angle0.now + leg->angle3)*sin(leg->angle0.now + leg->angle2)*sin(leg->angle3 - leg->angle4)),
+                      (leg->L0.now*cos(leg->angle0.now + leg->angle2)*sin(leg->angle2 - leg->angle3))/(l1*cos(leg->angle0.now + leg->angle2)*sin(leg->angle0.now + leg->angle3)*sin(leg->angle1 - leg->angle2) - l1*cos(leg->angle0.now + leg->angle3)*sin(leg->angle0.now + leg->angle2)*sin(leg->angle1 - leg->angle2)),
+                      -(leg->L0.now*cos(leg->angle0.now + leg->angle3)*sin(leg->angle2 - leg->angle3))/(l4*cos(leg->angle0.now + leg->angle2)*sin(leg->angle0.now + leg->angle3)*sin(leg->angle3 - leg->angle4) - l4*cos(leg->angle0.now + leg->angle3)*sin(leg->angle0.now + leg->angle2)*sin(leg->angle3 - leg->angle4))};
+
+  leg->Fnow = trans[0][0] * leg->TFnow + trans[0][1] * leg->TBnow;
+  leg->Tpnow = trans[1][0] * leg->TFnow + trans[1][1] * leg->TBnow;
+}
+
+void flyCheck(Leg* leg, float accely) {
+  INVMC(leg);
+  float zwdd = accely - leg->L0.ddot * cos(leg->angle0.now) + 2.0 * leg->angle0.dot * leg->L0.dot * sin(leg->angle0.now) + leg->L0.now * leg->angle0.ddot * sin(leg->angle0.now) + leg->L0.now * leg->angle0.dot * leg->angle0.dot * cos(leg->angle0.now);
+
+  // 这里的 F_set 本来应该是电机反馈力矩解算出来的，但是webots的电机反馈力矩非常的恶心
+  // Fnow
+  leg->normalforce = MASSWHEEL * zwdd + GRAVITY * MASSWHEEL + leg->Fnow * cos(leg->angle0.now) + leg->TWheelnow * sin(leg->angle0.now) / leg->L0.now;
+  if (leg->normalforce > -20) {
+    leg->flyflag = true;
+  }
+  else {
+    leg->flyflag = false;
+  }
+}

Chassis/src/robot.c

@@ -0,0 +1,56 @@
+#include "robot.h"
+
+Robot* robot;
+
+//----
+// @brief 初始化
+// 
+//----
+void robotInit() {
+  robot = (Robot *) malloc(sizeof(Robot));
+  legInit(&robot->legL, LEFT);
+  legInit(&robot->legR, RIGHT);
+  legInit(&robot->legSim, LEFT);
+  // TODO: 参数暂定
+  pidInit(&robot->yawpid, 1, 1, 1, 0, 0, PIDPOS);
+  pidInit(&robot->rollpid, 1, 1, 1, 0, 0, PIDPOS);
+  pidInit(&robot->splitpid, 1, 1, 1, 0, 0, PIDPOS);
+  robot->yawpid.target = 0;
+  robot->rollpid.target = 0;
+  robot->splitpid.target = 0;
+}
+
+//----
+// @brief 状态量更新
+// 
+//----
+void updateState() {
+  robot->yawpid.real = robot->yesense.yaw.now;
+  robot->rollpid.real = robot->yesense.roll.now;
+  Zjie(&robot->legL, robot->yesense.pitch.now);
+  Zjie(&robot->legR, robot->yesense.pitch.now);
+
+}
+
+//----
+// @brief lqr 控制保持平衡
+// 
+//----
+void balanceMode() {
+}
+
+//----
+// @brief 跳跃 可以先用简单的位控来实现跳跃
+// 
+//----
+void jumpMode() {
+
+}
+
+//----
+// @brief 腾空模式，但是感觉可以与平衡模式写到一起
+// 
+//----
+void flyMode() {
+
+}

Chassis/src/robotparam.c

@@ -0,0 +1,15 @@
+#include "robotparam.h"
+
+void inputInit(Input* input) {
+  input->theta = 0;
+  input->thetadot = 0;
+  input->x = 0;
+  input->v = 0;
+  input->pitch = 0;
+  input->pitchdot = 0;
+}
+
+void outputInit(Output* output) {
+  output->Tp = 0;
+  output->Twheel = 0;
+}