123 lines
3.4 KiB
C++
123 lines
3.4 KiB
C++
#include "module/arm.hpp"
|
|
#include "bsp/time.h"
|
|
|
|
Arm::Arm(){
|
|
dm_motor_params[0] = MOTOR_DM_Param_t{
|
|
.can = BSP_CAN_1,
|
|
.master_id = 0x14,
|
|
.can_id = 0x04,
|
|
.module = MOTOR_DM_J4310,
|
|
.reverse = false
|
|
};
|
|
dm_motor_params[1] = MOTOR_DM_Param_t{
|
|
.can = BSP_CAN_1,
|
|
.master_id = 0x15,
|
|
.can_id = 0x05,
|
|
.module = MOTOR_DM_J4310,
|
|
.reverse = false
|
|
};
|
|
dm_motor_params[2] = MOTOR_DM_Param_t{
|
|
.can = BSP_CAN_1,
|
|
.master_id = 0x16,
|
|
.can_id = 0x06,
|
|
.module = MOTOR_DM_J4310,
|
|
.reverse = false
|
|
};
|
|
lz_motor_params[0] = MOTOR_LZ_Param_t{
|
|
.can = BSP_CAN_1,
|
|
.motor_id = 127,
|
|
.host_id = 0xff,
|
|
.module= MOTOR_LZ_RSO3,
|
|
.reverse = false,
|
|
.mode = MOTOR_LZ_MODE_MOTION
|
|
};
|
|
lz_motor_params[1] = MOTOR_LZ_Param_t{
|
|
.can = BSP_CAN_1,
|
|
.motor_id = 126,
|
|
.host_id = 0xff,
|
|
.module= MOTOR_LZ_RSO3,
|
|
.reverse = false,
|
|
.mode = MOTOR_LZ_MODE_MOTION
|
|
};
|
|
lz_motor_params[2] = MOTOR_LZ_Param_t{
|
|
.can = BSP_CAN_1,
|
|
.motor_id = 125,
|
|
.host_id = 0xff,
|
|
.module= MOTOR_LZ_RSO3,
|
|
.reverse = false,
|
|
.mode = MOTOR_LZ_MODE_MOTION
|
|
};
|
|
}
|
|
|
|
void Arm::init(){
|
|
MOTOR_LZ_Init();
|
|
MOTOR_LZ_Register(&lz_motor_params[0]);
|
|
MOTOR_LZ_Enable(&lz_motor_params[0]);
|
|
BSP_TIME_Delay(2);
|
|
MOTOR_LZ_Register(&lz_motor_params[1]);
|
|
MOTOR_LZ_Enable(&lz_motor_params[1]);
|
|
BSP_TIME_Delay(2);
|
|
MOTOR_LZ_Register(&lz_motor_params[2]);
|
|
MOTOR_LZ_Enable(&lz_motor_params[2]);
|
|
BSP_TIME_Delay(2);
|
|
|
|
MOTOR_DM_Register(&dm_motor_params[0]);
|
|
MOTOR_DM_Enable(&dm_motor_params[0]);
|
|
BSP_TIME_Delay(2);
|
|
MOTOR_DM_Register(&dm_motor_params[1]);
|
|
MOTOR_DM_Enable(&dm_motor_params[1]);
|
|
BSP_TIME_Delay(2);
|
|
MOTOR_DM_Register(&dm_motor_params[2]);
|
|
MOTOR_DM_Enable(&dm_motor_params[2]);
|
|
BSP_TIME_Delay(2);
|
|
|
|
}
|
|
|
|
void Arm::operator()(){
|
|
MOTOR_DM_Relax(&dm_motor_params[0]);
|
|
BSP_TIME_Delay(2);
|
|
MOTOR_DM_Relax(&dm_motor_params[1]);
|
|
BSP_TIME_Delay(2);
|
|
MOTOR_DM_Relax(&dm_motor_params[2]);
|
|
BSP_TIME_Delay(2);
|
|
|
|
MOTOR_DM_UpdateAll();
|
|
|
|
MOTOR_DM_t *motor_dm = NULL;
|
|
motor_dm = MOTOR_DM_GetMotor(&dm_motor_params[0]);
|
|
arm_dm_motor[0] = *motor_dm;
|
|
motor_dm = MOTOR_DM_GetMotor(&dm_motor_params[1]);
|
|
arm_dm_motor[1] = *motor_dm;
|
|
motor_dm = MOTOR_DM_GetMotor(&dm_motor_params[2]);
|
|
arm_dm_motor[2] = *motor_dm;
|
|
|
|
arm_output[0].angle = 1.0f;
|
|
arm_output[0].velocity = 1.0f;
|
|
arm_output[0].kp = 0.5f;
|
|
arm_output[0].kd = 1.0f;
|
|
// MOTOR_DM_MITCtrl(&dm_motor_params[0], &arm_output[0]);
|
|
|
|
MOTOR_LZ_Relax(&lz_motor_params[0]);
|
|
BSP_TIME_Delay(2);
|
|
MOTOR_LZ_Relax(&lz_motor_params[1]);
|
|
BSP_TIME_Delay(2);
|
|
MOTOR_LZ_Relax(&lz_motor_params[2]);
|
|
BSP_TIME_Delay(2);
|
|
|
|
MOTOR_LZ_UpdateAll();
|
|
|
|
MOTOR_LZ_t *motor_lz = NULL;
|
|
motor_lz = MOTOR_LZ_GetMotor(&lz_motor_params[0]);
|
|
arm_lz_motor[0] = *motor_lz;
|
|
motor_lz = MOTOR_LZ_GetMotor(&lz_motor_params[1]);
|
|
arm_lz_motor[1] = *motor_lz;
|
|
motor_lz = MOTOR_LZ_GetMotor(&lz_motor_params[2]);
|
|
arm_lz_motor[2] = *motor_lz;
|
|
// arm_lz_motion[0].target_angle = 1.0f;
|
|
arm_lz_motion[0].target_velocity = 1.0f;
|
|
// arm_lz_motion[0].kp = 0.5f;
|
|
arm_lz_motion[0].kd = 0.5f;
|
|
// MOTOR_LZ_MotionControl(&lz_motor_params[0], &arm_lz_motion[0]);
|
|
|
|
}
|