42 lines
1013 B
C++
42 lines
1013 B
C++
#pragma once
|
|
|
|
#include "main.h"
|
|
#include "bsp/can.h"
|
|
#include "device/motor.h"
|
|
#include "device/motor_lz.h"
|
|
#include "device/motor_rm.h"
|
|
#include "device/motor_dm.h"
|
|
#include "component/pid.h"
|
|
|
|
class Arm {
|
|
public:
|
|
Arm();
|
|
void init();
|
|
void operator()();
|
|
private:
|
|
void read_adta();
|
|
void calc_joint_angle();
|
|
void calc_motor_angle();
|
|
void send_control();
|
|
void arm_relax();
|
|
void calc_pid();
|
|
float joint_angle[6] = {0}; //关节角度
|
|
// float motor_angle[6] = {0}; //电机角度
|
|
MOTOR_DM_Param_t dm_motor_params[3];
|
|
MOTOR_DM_t arm_dm_motor[3];
|
|
MOTOR_LZ_Param_t lz_motor_params[3];
|
|
MOTOR_LZ_t arm_lz_motor[3];
|
|
MOTOR_LZ_MotionParam_t arm_lz_output[3];
|
|
MOTOR_MIT_Output_t arm_dm_output[3];
|
|
KPID_Params_t pid_params[6];
|
|
KPID_Params_t pid_v_params[6];
|
|
float output[6] = {0};
|
|
KPID_t pid[6];
|
|
KPID_t pid_v[6];
|
|
float current_pos[6];
|
|
float current_vel[6];
|
|
uint64_t time = 0;
|
|
uint64_t last_time = 0;
|
|
uint64_t dt = 0;
|
|
|
|
}; |