Skip to content

Commit

Permalink
KF
Browse files Browse the repository at this point in the history
  • Loading branch information
@Luo25177
Luo25177 committed Mar 16, 2024
1 parent 20a7539 commit 56c9e31
Show file tree
Hide file tree
Showing 27 changed files with 635 additions and 131 deletions.
1 change: 1 addition & 0 deletions Chassis/inc/leg.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#pragma once

#include "DataStruct.h"
#include "btoscilloscope.h"
#include "robotparam.h"
#include "tim.h"
#include "tmotor.h"
Expand Down
1 change: 1 addition & 0 deletions Chassis/inc/robot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,3 +46,4 @@ void Jump();
void HaltMode();
void FlyCheck();
void RobotRun();
void WBCControl();
1 change: 1 addition & 0 deletions Chassis/inc/robotmonitor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,3 +8,4 @@ void LegRPidMonitor();
void RobotLqrMonitor();
void RobotTorqueMonitor();
void RobotWholeDataMonitor();
void RobotThetaMonitor();
13 changes: 13 additions & 0 deletions Chassis/inc/robotparam.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,20 @@ typedef struct {
float Twheel;
} Output;

typedef struct {
float s;
float sdot;
float psi;
float psidot;
float thetal;
float thetaldot;
} WBCInput;

typedef struct {
} WBCOutput;

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

extern float Kcoeff[12][4];
extern float Kcoeff_wbc[40][6];
40 changes: 25 additions & 15 deletions Chassis/src/leg.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,16 +80,18 @@ void Zjie(Leg* leg, float pitch) {
cor_XY_then[1] = sin(pitch) * leg->xc + cos(pitch) * leg->yc;
leg->theta.now = atan2f(cor_XY_then[0], cor_XY_then[1]);

float dt = 10000 / (float) (GlobalTimer - leg->timer);
float dt = (float) (GlobalTimer - leg->timer) / 10000;
leg->timer = GlobalTimer;

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.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->theta.dot = (leg->theta.now - leg->theta.last) * dt;
leg->theta.ddot = (leg->theta.dot - leg->theta.lastdot) * dt;
leg->theta.dot = (leg->theta.now - leg->theta.last) / dt;
leg->theta.ddot = (leg->theta.dot - leg->theta.lastdot) / dt;

leg->theta.last = leg->theta.now;
leg->theta.lastdot = leg->theta.dot;
}
Expand Down Expand Up @@ -123,12 +125,16 @@ void Njie(Leg* leg, float xc, float yc) {
// @param leg
//----
void VMC(Leg* leg) {
float trans[2][2] = { l1 * sin(leg->angle0 - leg->angle3) * sin(leg->angle1 - leg->angle2) / sin(leg->angle2 - leg->angle3),
l1 * cos(leg->angle0 - leg->angle3) * sin(leg->angle1 - leg->angle2) / (leg->L0.now * sin(leg->angle2 - leg->angle3)),
l4 * sin(leg->angle0 - leg->angle2) * sin(leg->angle3 - leg->angle4) / sin(leg->angle2 - leg->angle3),
l4 * cos(leg->angle0 - leg->angle2) * sin(leg->angle3 - leg->angle4) / (leg->L0.now * sin(leg->angle2 - leg->angle3)) };
leg->TFset = trans[0][0] * leg->Fset - trans[0][1] * leg->Tpset;
leg->TBset = trans[1][0] * leg->Fset - trans[1][1] * leg->Tpset;
float A = l1 * sin(leg->angle1 - leg->angle2) / sin(leg->angle2 - leg->angle3);
float B = l4 * sin(leg->angle3 - leg->angle4) / sin(leg->angle2 - leg->angle3);

float trans[2][2] = { -A * cos(leg->theta.now + leg->angle3),
A * sin(leg->theta.now + leg->angle3) / leg->L0.now,
-B * cos(leg->theta.now + leg->angle2),
B * sin(leg->theta.now + leg->angle2) / leg->L0.now };

leg->TFset = -trans[0][0] * leg->Fset - trans[0][1] * leg->Tpset;
leg->TBset = -trans[1][0] * leg->Fset - trans[1][1] * leg->Tpset;
}

//----
Expand All @@ -137,11 +143,15 @@ void VMC(Leg* leg) {
// @param leg
//----
void INVMC(Leg* leg) {
float A = sin(leg->angle2 - leg->angle3) / (l1 * cos(leg->angle0 - leg->angle2) * sin(leg->angle0 - leg->angle3) * sin(leg->angle1 - leg->angle2) - l1 * cos(leg->angle0 - leg->angle3) * sin(leg->angle0 - leg->angle2) * sin(leg->angle1 - leg->angle2));
float B = sin(leg->angle2 - leg->angle3) / (l4 * cos(leg->angle0 - leg->angle2) * sin(leg->angle0 - leg->angle3) * sin(leg->angle3 - leg->angle4) - l4 * cos(leg->angle0 - leg->angle3) * sin(leg->angle0 - leg->angle2) * sin(leg->angle3 - leg->angle4));
float A = l1 * sin(leg->angle1 - leg->angle2) / sin(leg->angle2 - leg->angle3);
float B = l4 * sin(leg->angle3 - leg->angle4) / sin(leg->angle2 - leg->angle3);

A = A * cos(leg->angle2 + leg->theta.now) * sin(leg->angle3 + leg->theta.now) - A * cos(leg->angle3 + leg->theta.now) * sin(leg->angle2 + leg->theta.now);
B = B * cos(leg->angle2 + leg->theta.now) * sin(leg->angle3 + leg->theta.now) - B * cos(leg->angle3 + leg->theta.now) * sin(leg->angle2 + leg->theta.now);

float trans[2][2] = { cos(leg->angle0 - leg->angle2) * A, -cos(leg->angle0 - leg->angle3) * B, -leg->L0.now * sin(leg->angle0 - leg->angle2) * A, leg->L0.now * sin(leg->angle0 - leg->angle3) * B };
float trans[2][2] = { sin(leg->angle2 + leg->theta.now) / A, -sin(leg->angle3 + leg->theta.now) / B,
leg->L0.now * cos(leg->angle2 + leg->theta.now) / B, -leg->L0.now * cos(leg->angle3 + leg->theta.now) / B };

leg->Fnow = trans[0][0] * leg->TFnow + trans[0][1] * leg->TBnow;
leg->Fnow = -trans[0][0] * leg->TFnow - trans[0][1] * leg->TBnow;
leg->Tpnow = -trans[1][0] * leg->TFnow - trans[1][1] * leg->TBnow;
}
177 changes: 163 additions & 14 deletions Chassis/src/robot.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,9 @@
Robot robot;
Tmotor tmotor[4];
Zdrive zdrive[2];
float expectx = 0;
float expectx = 0;
// TODO:
float expecttheta = -1.5f;

//----
// @brief 初始化
Expand All @@ -30,7 +32,7 @@ void RobotInit() {
PidInit(&robot.rollpid, 1, 1, 1, 0, 1000, PIDPOS);
PidInit(&robot.splitpid, 120, 0, 1000, 0, 1000, PIDPOS);

robot.L0Set = 0.18;
robot.L0Set = 0.15;
robot.yawpid.target = 0;
robot.rollpid.target = 0;
robot.splitpid.target = 0;
Expand All @@ -45,19 +47,32 @@ void RobotInit() {
void UpdateState() {
LegUpdate(&robot.legL);
LegUpdate(&robot.legR);
Zjie(&robot.legL, robot.yesense.pitch.now);
Zjie(&robot.legR, robot.yesense.pitch.now);
float pitch = robot.yesense.pitch.now;
Zjie(&robot.legL, pitch);
Zjie(&robot.legR, pitch);

robot.legVir.dis.now = (-robot.legL.dis.now + robot.legR.dis.now) / 2;
robot.legVir.dis.dot = (-robot.legL.dis.dot + robot.legR.dis.dot) / 2;
robot.legVir.angle1 = (robot.legL.angle1 + robot.legR.angle1) / 2;
robot.legVir.angle4 = (robot.legL.angle4 + robot.legR.angle4) / 2;

Zjie(&robot.legVir, robot.yesense.pitch.now);
Zjie(&robot.legVir, pitch);

FlyCheck();
}

float x = 0;
float v = 0;
float r = 1;
//----
// @brief 微分跟踪器，使得跟踪值更加平滑
//
// @param refx
//----
void LTD(float refx) {
x = x + v * 0.0035;
v = v + 0.0035 * (-r * r * x - 2 * r * v + r * r * refx);
}
//----
// @brief lqr 控制保持平衡
//
Expand Down Expand Up @@ -85,16 +100,19 @@ void BalanceMode() {
}
}
}
robot.legVir.X.theta = robot.legVir.theta.now;
robot.legVir.X.thetadot = robot.legVir.theta.dot;
robot.legVir.X.x = robot.legVir.dis.now;
robot.legVir.X.v = robot.legVir.dis.dot;
robot.legVir.X.pitch = robot.yesense.pitch.now;
robot.legVir.X.pitchdot = robot.yesense.pitch.dot;

robot.legVir.Xd.theta = 0;
robot.legVir.X.theta = robot.legVir.theta.now;
robot.legVir.X.thetadot = robot.legVir.theta.dot;
LimitInRange(float)(&robot.legVir.X.thetadot, 5);
robot.legVir.X.x = robot.legVir.dis.now;
robot.legVir.X.v = robot.legVir.dis.dot;
robot.legVir.X.pitch = robot.yesense.pitch.now;
robot.legVir.X.pitchdot = robot.yesense.pitch.dot;

LTD(expectx);

robot.legVir.Xd.theta = expecttheta / 180 * PI;
robot.legVir.Xd.thetadot = 0;
robot.legVir.Xd.x = expectx;
robot.legVir.Xd.x = x;
robot.legVir.Xd.v = 0;
robot.legVir.Xd.pitch = 0;
robot.legVir.Xd.pitchdot = 0;
Expand Down Expand Up @@ -239,6 +257,7 @@ void FlyCheck() {
void RobotRun() {
switch (robot.mode) {
case ROBOTNORMAL:
// WBCControl();
BalanceMode();
break;
case ROBOTHALT:
Expand All @@ -255,3 +274,133 @@ void RobotRun() {
ZdriveRun(zdrive);
ZdriveRun(zdrive + 1);
}

void WBCControl() {
float L_l = robot.legL.L0.now;
float L_r = robot.legR.L0.now;
float L_l2 = L_l * L_l;
float L_r2 = L_r * L_r;
float L_lL_r = L_l * L_r;

float s = robot.legVir.dis.now;
float s_dot = robot.legVir.dis.dot;
float psi = robot.yesense.yaw.now;
float psi_dot = robot.yesense.yaw.dot;
float theta_l = robot.legL.theta.now;
float theta_l_dot = robot.legL.theta.dot;
float theta_r = robot.legR.theta.now;
float theta_r_dot = robot.legR.theta.dot;
float phi = robot.yesense.pitch.now;
float phi_dot = robot.yesense.pitch.dot;

float K[4][10];

if (robot.flyflag) {
for (int row = 0; row < 4; row++) {
for (int col = 0; col < 10; col++) {
int num = (row * 10) + col;
if ((row != 2 && row != 3) || (col != 4 && col != 5 && col != 6 && col != 7)) {
K[row][col] = 0;
continue;
}
K[row][col] = Kcoeff_wbc[num][0] + Kcoeff_wbc[num][1] * L_l + Kcoeff_wbc[num][2] * L_r + Kcoeff_wbc[num][3] * L_l2 +
Kcoeff_wbc[num][4] * L_r2 + Kcoeff_wbc[num][5] * L_lL_r;
}
}
} else {
for (int row = 0; row < 4; row++) {
for (int col = 0; col < 10; col++) {
int num = (row * 10) + col;
K[row][col] = Kcoeff_wbc[num][0] + Kcoeff_wbc[num][1] * L_l + Kcoeff_wbc[num][2] * L_r + Kcoeff_wbc[num][3] * L_l2 +
Kcoeff_wbc[num][4] * L_r2 + Kcoeff_wbc[num][5] * L_lL_r;
}
}
}

float Twl = K[0][0] * (0 - s) +
K[0][1] * (0 - s_dot) +
K[0][2] * (0 - psi) +
K[0][3] * (0 - psi_dot) +
K[0][4] * (0 - theta_l) +
K[0][5] * (0 - theta_l_dot) +
K[0][6] * (0 - theta_r) +
K[0][7] * (0 - theta_r_dot) +
K[0][8] * (0 - phi) +
K[0][9] * (0 - phi_dot);

float Twr = K[1][0] * (0 - s) +
K[1][1] * (0 - s_dot) +
K[1][2] * (0 - psi) +
K[1][3] * (0 - psi_dot) +
K[1][4] * (0 - theta_l) +
K[1][5] * (0 - theta_l_dot) +
K[1][6] * (0 - theta_r) +
K[1][7] * (0 - theta_r_dot) +
K[1][8] * (0 - phi) +
K[1][9] * (0 - phi_dot);

float Tbl = K[2][0] * (0 - s) +
K[2][1] * (0 - s_dot) +
K[2][2] * (0 - psi) +
K[2][3] * (0 - psi_dot) +
K[2][4] * (0 - theta_l) +
K[2][5] * (0 - theta_l_dot) +
K[2][6] * (0 - theta_r) +
K[2][7] * (0 - theta_r_dot) +
K[2][8] * (0 - phi) +
K[2][9] * (0 - phi_dot);

float Tbr = K[3][0] * (0 - s) +
K[3][1] * (0 - s_dot) +
K[3][2] * (0 - psi) +
K[3][3] * (0 - psi_dot) +
K[3][4] * (0 - theta_l) +
K[3][5] * (0 - theta_l_dot) +
K[3][6] * (0 - theta_r) +
K[3][7] * (0 - theta_r_dot) +
K[3][8] * (0 - phi) +
K[3][9] * (0 - phi_dot);

robot.legL.TWheelset = Twl;
robot.legR.TWheelset = Twr;

robot.legL.Tpset = Tbl;
robot.legR.Tpset = Tbr;

// 前馈力
robot.legL.Fset = FFEEDFORWARD;
robot.legR.Fset = FFEEDFORWARD;
// 补偿虚拟力
float lfCompensate = robot.legL.L0pid.compute(&robot.legL.L0pid, robot.legL.L0.now);
float rfCompensate = robot.legR.L0pid.compute(&robot.legR.L0pid, robot.legR.L0.now);
robot.legL.Fset += lfCompensate;
robot.legR.Fset += rfCompensate;
// 劈腿补偿
float splitCompensate = robot.splitpid.compute(&robot.splitpid, robot.legL.theta.now - robot.legR.theta.now);
robot.legL.Tpset += splitCompensate;
robot.legR.Tpset -= splitCompensate;
// 翻滚角补偿
// float rollCompensate = 0; // robot.rollpid->compute(robot.rollpid, robot.yesense.roll.now);
// robot.legL.Fset -= rollCompensate;
// robot.legR.Fset += rollCompensate;

VMC(&robot.legL);
VMC(&robot.legR);

// 方向
// robot.legL.TWheelset *= robot.legL.dir;
robot.legL.TFset *= robot.legL.dir;
robot.legL.TBset *= robot.legL.dir;

robot.legR.TWheelset *= robot.legR.dir;
robot.legR.TFset *= robot.legR.dir;
robot.legR.TBset *= robot.legR.dir;

robot.legR.front->set.torque = robot.legR.TFset;
robot.legR.behind->set.torque = robot.legR.TBset;
robot.legR.wheel->set.torque = robot.legR.TWheelset;

robot.legL.front->set.torque = robot.legL.TFset;
robot.legL.behind->set.torque = robot.legL.TBset;
robot.legL.wheel->set.torque = robot.legL.TWheelset;
}
37 changes: 26 additions & 11 deletions Chassis/src/robotmonitor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,17 +24,18 @@ void LegRPidMonitor() {

void RobotLqrMonitor() {
float data[12];
data[0] = robot.legVir.X.theta;
data[1] = robot.legVir.X.thetadot;
data[2] = robot.legVir.X.x;
data[3] = robot.legVir.X.v;
data[4] = robot.legVir.X.pitch;
data[5] = robot.legVir.X.pitchdot;
data[6] = robot.legL.wheel->set.torque;
data[7] = robot.legR.wheel->set.torque;
data[8] = robot.legL.wheel->real.torque;
data[9] = robot.legR.wheel->real.torque;
Oscilloscope(data, 10);
data[0] = robot.legVir.X.theta;
data[1] = robot.legVir.X.thetadot;
data[2] = robot.legVir.X.x;
data[3] = robot.legVir.X.v;
data[4] = robot.legVir.X.pitch;
data[5] = robot.legVir.X.pitchdot;
data[6] = robot.legL.wheel->set.torque;
data[7] = robot.legR.wheel->set.torque;
data[8] = robot.legL.wheel->real.torque;
data[9] = robot.legR.wheel->real.torque;
data[10] = robot.legVir.Xd.x;
Oscilloscope(data, 11);
}

void RobotTorqueMonitor() {
Expand Down Expand Up @@ -64,3 +65,17 @@ void RobotWholeDataMonitor() {
data[11] = robot.legL.TWheelset;
Oscilloscope(data, 12);
}

void RobotThetaMonitor(){
float data[8];
data[0] = robot.legL.theta.now;
data[1] = robot.legL.theta.last;
data[2] = robot.legL.theta.dot;
data[3] = robot.legL.theta.lastdot;
data[4] = robot.legR.theta.now;
data[5] = robot.legR.theta.last;
data[6] = robot.legR.theta.dot;
data[7] = robot.legR.theta.lastdot;
Oscilloscope(data, 8);
}

Loading

0 comments on commit 56c9e31

Please sign in to comment.