Skip to content

Commit

Permalink
implement position control
Browse files Browse the repository at this point in the history
  • Loading branch information
@alvinsunyixiao
alvinsunyixiao committed Mar 13, 2024
1 parent 0977d9d commit 5539c0f
Show file tree
Hide file tree
Showing 4 changed files with 250 additions and 11 deletions.
37 changes: 31 additions & 6 deletions shared/libraries/cybergear.cc
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
#include "bsp_os.h"
#include "cybergear.h"
#include <cstring>

Expand Down Expand Up @@ -96,13 +97,15 @@ static void cybergear_callback(const uint8_t data[], const CAN_RxHeaderTypeDef&
}

void CyberGear::UpdateData(const uint8_t data[], const CAN_RxHeaderTypeDef& header) {
angle_=uint16_to_float(data[0]<<8 | data[1], MIN_P, MAX_P, 16);
speed_=uint16_to_float(data[2]<<8 | data[3], V_MIN, V_MAX, 16);
torque_=uint16_to_float(data[4]<<8 | data[5], T_MIN, T_MAX, 16);
temp_=(data[6]<<8|data[7])*Temp_Gain;
timestamp_ = bsp::GetHighresTickMicroSec();

master_can_id_ = header.ExtId & 0xFF;
error_code_ = (header.ExtId & 0x1F0000)>>16;
angle_=uint16_to_float(data[0]<<8 | data[1], P_MIN, P_MAX, 16);
speed_=uint16_to_float(data[2]<<8 | data[3], V_MIN, V_MAX, 16);
torque_=uint16_to_float(data[4]<<8 | data[5], T_MIN, T_MAX, 16);
temp_=(data[6]<<8|data[7])*Temp_Gain;

master_can_id_ = header.ExtId & 0xFF;
error_code_ = (header.ExtId & 0x1F0000)>>16;
}

/**
Expand Down Expand Up @@ -259,6 +262,24 @@ void CyberGear::SendCurrentCommand(float current) {
SetMotorParameter(Iq_Ref, current);
}

void CyberGear::SendPositionCommand(float position, float max_speed, float max_current) {
SetMotorParameter(Limit_Cur, max_current);
SetMotorParameter(Limit_Spd, max_speed);
SetMotorParameter(Loc_Ref, position);
}

void CyberGear::SetPositionKp(float kp) {
SetMotorParameter(Loc_Kp, kp);
}

void CyberGear::SetSpeedKp(float kp) {
SetMotorParameter(Spd_Kp, kp);
}

void CyberGear::SetSpeedKi(float kp) {
SetMotorParameter(Spd_Ki, kp);
}

float CyberGear::GetAngle() const {
return angle_;
}
Expand All @@ -279,4 +300,8 @@ uint8_t CyberGear::GetMasterCanID() const {
return master_can_id_;
}

uint32_t CyberGear::GetTimeStamp() const {
return timestamp_;
}

} // namespace xiaomi
16 changes: 14 additions & 2 deletions shared/libraries/cybergear.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@
@endverbatim
****************************(C)SWJTU_ROBOTCON****************************
**/
#pragma once

#include "bsp_can.h"

//控制参数最值，谨慎更改
Expand All @@ -26,8 +28,8 @@
#define KD_MAX 5.0f
#define T_MIN -12.0f
#define T_MAX 12.0f
#define MAX_P 720
#define MIN_P -720
// #define MAX_P 720
// #define MIN_P -720
//主机CANID设置
#define Master_CAN_ID 0x00 //主机ID
//控制命令宏定义
Expand All @@ -43,6 +45,9 @@
#define Communication_Type_SetSingleParameter 0x12 //设定单个参数
#define Communication_Type_ErrorFeedback 0x15 //故障反馈帧
//参数读取宏定义
#define Spd_Kp 0x2014
#define Spd_Ki 0x2015
#define Loc_Kp 0x2016
#define Run_mode 0x7005
#define Iq_Ref 0x7006
#define Spd_Ref 0x700A
Expand Down Expand Up @@ -101,6 +106,10 @@ class CyberGear { //小米电机结构体
void SetMode(const control_mode_t &mode);
void SendMotionCommand(float torque, float position, float speed, float kp, float kd);
void SendCurrentCommand(float current);
void SendPositionCommand(float position, float max_speed = 1.0, float max_current = 23.0);
void SetPositionKp(float kp);
void SetSpeedKp(float kp);
void SetSpeedKi(float ki);
void SetZeroPosition();
void UpdateData(const uint8_t data[], const CAN_RxHeaderTypeDef& header);

Expand All @@ -109,6 +118,7 @@ class CyberGear { //小米电机结构体
float GetTorque() const;
float GetTemperature() const;
uint8_t GetMasterCanID() const;
uint32_t GetTimeStamp() const;

private:
void SetMotorParameter(uint16_t index, float value);
Expand All @@ -123,6 +133,8 @@ class CyberGear { //小米电机结构体
float torque_; //回传力矩
float temp_; //回传温度

uint32_t timestamp_;

uint8_t master_can_id_;
uint8_t error_code_;
};
Expand Down
3 changes: 2 additions & 1 deletion vehicles/MultiCyberGear/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,5 +22,6 @@ project(MultiCyberGear ASM C CXX)

irm_add_arm_executable(multi_cybergear
TARGET DJI_Board_TypeC
SOURCES main.cc)
SOURCES main.cc
DEPENDS eigen3)

205 changes: 203 additions & 2 deletions vehicles/MultiCyberGear/main.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,176 @@
* *
****************************************************************************/

#include <cmath>

#include "bsp_print.h"
#include "bsp_os.h"
#include "bsp_gpio.h"
#include "cmsis_os2.h"
#include "main.h"

#include "cybergear.h"

#define MOTOR_BASE_ID 126
#define MOTOR_ARM1_ID 127

#define MOTOR_BASE_TARGET_ANGLE (-0.4049)
#define MOTOR_ARM1_TARGET_ANGLE (-1.785)

#define KP 5.0
#define KD 0.6

#define MOTOR_WEIGHT 0.317
#define ARM1_LENGTH 0.13
#define GRAVITY 9.8

#include <Eigen/Dense>

namespace sym {

/**
* This function was autogenerated. Do not modify by hand.
*
* Args:
* t1: Scalar
* tdot1: Scalar
* t2: Scalar
* tdot2: Scalar
* m: Scalar
* dt: Scalar
* l: Scalar
* g: Scalar
* tau1: Scalar
* tau2: Scalar
*
* Outputs:
* f: Matrix51
* A_mat: Matrix55
* B_mat: Matrix52
*/
template <typename Scalar>
void Forward(const Scalar t1, const Scalar tdot1, const Scalar t2, const Scalar tdot2,
const Scalar m, const Scalar dt, const Scalar l, const Scalar g, const Scalar tau1,
const Scalar tau2, Eigen::Matrix<Scalar, 5, 1>* const f = nullptr,
Eigen::Matrix<Scalar, 5, 5>* const A_mat = nullptr,
Eigen::Matrix<Scalar, 5, 2>* const B_mat = nullptr) {
// Total ops: 44

// Input arrays

// Intermediate terms (13)
const Scalar _tmp0 = std::pow(l, Scalar(2));
const Scalar _tmp1 = _tmp0 * m;
const Scalar _tmp2 = std::sin(t2);
const Scalar _tmp3 = std::pow(_tmp2, Scalar(2)) * dt;
const Scalar _tmp4 = _tmp1 * _tmp3 + Scalar(1.0000000000000001e-9);
const Scalar _tmp5 = Scalar(1.0) / (_tmp4);
const Scalar _tmp6 = _tmp2 * g * l * m + tau2;
const Scalar _tmp7 = Scalar(1.0) / (m);
const Scalar _tmp8 = dt / _tmp0;
const Scalar _tmp9 = _tmp7 * _tmp8;
const Scalar _tmp10 = dt * std::cos(t2);
const Scalar _tmp11 = tau1 / std::pow(_tmp4, Scalar(2));
const Scalar _tmp12 = g / l;

// Output terms (3)
if (f != nullptr) {
Eigen::Matrix<Scalar, 5, 1>& _f = (*f);

_f(0, 0) = dt * tdot1 + t1;
_f(1, 0) = _tmp5 * tau1 + tdot1;
_f(2, 0) = dt * tdot2 + t2;
_f(3, 0) = _tmp6 * _tmp9 + tdot2;
_f(4, 0) = m;
}

if (A_mat != nullptr) {
Eigen::Matrix<Scalar, 5, 5>& _A_mat = (*A_mat);

_A_mat.setZero();

_A_mat(0, 0) = 1;
_A_mat(0, 1) = dt;
_A_mat(1, 1) = 1;
_A_mat(1, 2) = -2 * _tmp1 * _tmp10 * _tmp11 * _tmp2;
_A_mat(2, 2) = 1;
_A_mat(3, 2) = _tmp10 * _tmp12;
_A_mat(2, 3) = dt;
_A_mat(3, 3) = 1;
_A_mat(1, 4) = -_tmp0 * _tmp11 * _tmp3;
_A_mat(3, 4) = _tmp12 * _tmp2 * _tmp7 * dt - _tmp6 * _tmp8 / std::pow(m, Scalar(2));
_A_mat(4, 4) = 1;
}

if (B_mat != nullptr) {
Eigen::Matrix<Scalar, 5, 2>& _B_mat = (*B_mat);

_B_mat.setZero();

_B_mat(1, 0) = _tmp5;
_B_mat(3, 1) = _tmp9;
}
} // NOLINT(readability/fn_size)

// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym


class KalmanFilter {
public:
KalmanFilter() {
// construct C
C.col(4).setZero();
C.block<4, 4>(0, 0).setIdentity();

// construct Q
Q.setZero();
Q(0, 0) = 1e-4;
Q(1, 1) = 1e-4;
Q(2, 2) = 1e-4;
Q(3, 3) = 1e-4;
Q(4, 4) = 1e-8;

// construct R
R.setZero();
R(0, 0) = 1e-4;
R(1, 1) = 1e-4;
R(2, 2) = 1e-4;
R(3, 3) = 1e-4;
}

void Update(double tau1, double tau2, double theta1, double dtheta1, double theta2, double dtheta2, double dt) {
Eigen::Matrix<double, 5, 1> x_pred;
Eigen::Matrix<double, 5, 5> P_pred;
Eigen::Matrix<double, 5, 5> A;
Eigen::Matrix<double, 5, 2> B;

// prediction update
sym::Forward<double>(x[0], x[1], x[2], x[3], x[4], dt, ARM1_LENGTH, GRAVITY, tau1, tau2, &x_pred, &A, &B);
P_pred = A * P * A.transpose() + Q;

// measurement update
Eigen::Vector4d z(theta1, dtheta1, theta2, dtheta2);
Eigen::Vector4d innovation = z - x.block<4, 1>(0, 0);
innovation[0] = std::remainder(innovation[0], 2.0 * M_PI);
innovation[2] = std::remainder(innovation[2], 2.0 * M_PI);
Eigen::Matrix<double, 4, 4> S = C * P_pred * C.transpose() + R;
Eigen::Matrix<double, 5, 4> K = P_pred * C.transpose() * S.inverse();
x = x_pred + K * innovation;
P = P_pred - K * C * P_pred;
}

void MeasurementUpdate();

private:
Eigen::Matrix<double, 5, 1> x;
Eigen::Matrix<double, 5, 5> P;
Eigen::Matrix<double, 4, 5> C;
Eigen::Matrix<double, 5, 5> Q;
Eigen::Matrix<double, 4, 4> R;
};


const osTimerAttr_t controlTimerAttribute = {
.name = "controlTimer",
.attr_bits = 0,
Expand All @@ -32,10 +197,21 @@ const osTimerAttr_t controlTimerAttribute = {

osTimerId_t controlTimer;

static xiaomi::CAN* can = nullptr;
static xiaomi::CyberGear* motor_base = nullptr;
static xiaomi::CyberGear* motor_arm1 = nullptr;
static bsp::GPIO* key = nullptr;

void controlTask(void* arg) {
UNUSED(arg);

print("%lu\r\n", osKernelGetTickCount());
if (key->Read()) {
motor_base->SendPositionCommand(0.0);
motor_arm1->SendPositionCommand(0.0);
} else {
motor_base->SendPositionCommand(0.1);
motor_arm1->SendPositionCommand(0.0);
}
}

//==================================================================================================
Expand All @@ -44,6 +220,13 @@ void controlTask(void* arg) {

void RM_RTOS_Init(void) {
print_use_usb();

bsp::SetHighresClockTimer(&htim5);

key = new bsp::GPIO(KEY_GPIO_Port, KEY_Pin);
can = new xiaomi::CAN(&hcan1);
motor_base = new xiaomi::CyberGear(can, MOTOR_BASE_ID, xiaomi::Position_mode);
motor_arm1 = new xiaomi::CyberGear(can, MOTOR_ARM1_ID, xiaomi::Position_mode);
}

void RM_RTOS_Timers_Init(void) {
Expand All @@ -57,7 +240,25 @@ void RM_RTOS_Timers_Init(void) {
void RM_RTOS_Default_Task(const void* arg) {
UNUSED(arg);

osTimerStart(controlTimer, 2U);
motor_base->SetZeroPosition();
motor_arm1->SetZeroPosition();

motor_base->SetSpeedKi(0.);
motor_arm1->SetSpeedKi(0.);
motor_base->SetPositionKp(10.);
motor_arm1->SetPositionKp(10.);
motor_base->SetSpeedKp(1.);
motor_arm1->SetSpeedKp(1.);

osDelay(10);

osTimerStart(controlTimer, 1U);

while (true) {
print("arm1 torque: %.4f\r\n", motor_arm1->GetTorque());

osDelay(10);
}
}

//==================================================================================================
Expand Down

0 comments on commit 5539c0f

Please sign in to comment.