47 lines
1.1 KiB
C++
47 lines
1.1 KiB
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"
|
|
#include <cstdint>
|
|
|
|
class Arm {
|
|
public:
|
|
Arm();
|
|
void init();
|
|
void operator()();
|
|
private:
|
|
void read_data();
|
|
void calc_joint_angle();
|
|
void calc_motor_angle();
|
|
void send_control();
|
|
void arm_relax();
|
|
void arm_enable();
|
|
void calc_pid();
|
|
void change_mode();
|
|
float joint_angle[7] = {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[7];
|
|
KPID_Params_t pid_v_params[7];
|
|
float output[7] = {0};
|
|
KPID_t pid[7];
|
|
KPID_t pid_v[7];
|
|
float current_pos[7];
|
|
float current_vel[7];
|
|
uint64_t time = 0;
|
|
uint64_t last_time = 0;
|
|
uint64_t dt = 0;
|
|
MOTOR_RM_Param_t motor_dji_param;
|
|
MOTOR_RM_t arm_motor_dji;
|
|
uint8_t last_sw_l;
|
|
}; |