加入重力补偿和路径规划
This commit is contained in:
shentou
parent
bef6f60c34
commit
9b2f29f3ff
@ -85,6 +85,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
/home/shentou/workspace/DM/Drivers/CMSIS/DSP/Source/MatrixFunctions/MatrixFunctions.c
|
||||
/home/shentou/workspace/DM/Drivers/CMSIS/DSP/Source/FastMathFunctions/FastMathFunctions.c
|
||||
/home/shentou/workspace/DM/Drivers/CMSIS/DSP/Source/CommonTables/CommonTables.c
|
||||
/home/shentou/workspace/DM/Drivers/CMSIS/DSP/Source/StatisticsFunctions/StatisticsFunctions.c
|
||||
)
|
||||
|
||||
# Add include paths
|
||||
|
||||
@ -42,12 +42,20 @@
|
||||
void MX_GPIO_Init(void)
|
||||
{
|
||||
|
||||
GPIO_InitTypeDef GPIO_InitStruct = {0};
|
||||
|
||||
/* GPIO Ports Clock Enable */
|
||||
__HAL_RCC_GPIOB_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOA_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOD_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOH_CLK_ENABLE();
|
||||
|
||||
/*Configure GPIO pin : PA0 */
|
||||
GPIO_InitStruct.Pin = GPIO_PIN_0;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
|
||||
|
||||
}
|
||||
|
||||
/* USER CODE BEGIN 2 */
|
||||
|
||||
@ -35,15 +35,16 @@ Mcu.Name=STM32F407I(E-G)Hx
|
||||
Mcu.Package=UFBGA176
|
||||
Mcu.Pin0=PB5
|
||||
Mcu.Pin1=PA14
|
||||
Mcu.Pin10=VP_SYS_VS_tim14
|
||||
Mcu.Pin2=PA13
|
||||
Mcu.Pin3=PB6
|
||||
Mcu.Pin4=PD0
|
||||
Mcu.Pin5=PD1
|
||||
Mcu.Pin6=PH0-OSC_IN
|
||||
Mcu.Pin7=PH1-OSC_OUT
|
||||
Mcu.Pin8=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin9=VP_SYS_VS_tim14
|
||||
Mcu.PinsNb=10
|
||||
Mcu.Pin8=PA0-WKUP
|
||||
Mcu.Pin9=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.PinsNb=11
|
||||
Mcu.ThirdPartyNb=0
|
||||
Mcu.UserConstants=
|
||||
Mcu.UserName=STM32F407IGHx
|
||||
@ -72,6 +73,8 @@ NVIC.TIM8_TRG_COM_TIM14_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true
|
||||
NVIC.TimeBase=TIM8_TRG_COM_TIM14_IRQn
|
||||
NVIC.TimeBaseIP=TIM14
|
||||
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||
PA0-WKUP.Locked=true
|
||||
PA0-WKUP.Signal=GPIO_Input
|
||||
PA13.Mode=Serial_Wire
|
||||
PA13.Signal=SYS_JTMS-SWDIO
|
||||
PA14.Mode=Serial_Wire
|
||||
|
||||
@ -9,6 +9,7 @@
|
||||
#include "module/config.h"
|
||||
|
||||
|
||||
#define _constrain(in,min,max) (in<min)?min:((in>max)?max:in)
|
||||
|
||||
|
||||
|
||||
@ -45,13 +46,13 @@
|
||||
|
||||
//joint2
|
||||
double compute_l5q2(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
|
||||
return 0.21817*9.81*(-0.091578999999999994*sin(q2)*sin(q3 + 1.5707963267948966) - 0.29910000000000003*sin(q2)*cos(q3 + 1.5707963267948966) - 0.29910000000000003*sin(q3 + 1.5707963267948966)*cos(q2) + 0.091578999999999994*cos(q2)*cos(q3 + 1.5707963267948966) + 0.40400000000000003*cos(q2));
|
||||
return 0.54317*9.81*(-0.091578999999999994*sin(q2)*sin(q3 + 1.5707963267948966) - 0.29910000000000003*sin(q2)*cos(q3 + 1.5707963267948966) - 0.29910000000000003*sin(q3 + 1.5707963267948966)*cos(q2) + 0.091578999999999994*cos(q2)*cos(q3 + 1.5707963267948966) + 0.40400000000000003*cos(q2));
|
||||
}
|
||||
double compute_l4q2(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
|
||||
return 0.60215*9.81*(-0.091578999999999994*sin(q2)*sin(q3 + 1.5707963267948966) - 0.189273*sin(q2)*cos(q3 + 1.5707963267948966) - 0.189273*sin(q3 + 1.5707963267948966)*cos(q2) + 0.091578999999999994*cos(q2)*cos(q3 + 1.5707963267948966) + 0.40400000000000003*cos(q2));
|
||||
return 0.92715*9.81*(-0.091578999999999994*sin(q2)*sin(q3 + 1.5707963267948966) - 0.189273*sin(q2)*cos(q3 + 1.5707963267948966) - 0.189273*sin(q3 + 1.5707963267948966)*cos(q2) + 0.091578999999999994*cos(q2)*cos(q3 + 1.5707963267948966) + 0.40400000000000003*cos(q2));
|
||||
}
|
||||
double compute_l3q2(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
|
||||
return 0.72964*9.81*(-0.086979000000000001*sin(q2)*sin(q3 + 1.5707963267948966) + 0.086979000000000001*cos(q2)*cos(q3 + 1.5707963267948966) + 0.40400000000000003*cos(q2));
|
||||
return 1.05464*9.81*(-0.086979000000000001*sin(q2)*sin(q3 + 1.5707963267948966) + 0.086979000000000001*cos(q2)*cos(q3 + 1.5707963267948966) + 0.40400000000000003*cos(q2));
|
||||
}
|
||||
double compute_l2q2(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
|
||||
return 1.1252*9.81*0.26277*cos(q2);
|
||||
@ -59,23 +60,23 @@ double compute_l2q2(double q1, double q2, double q3, double q4, double q5,double
|
||||
|
||||
//joint3
|
||||
double compute_l5q3(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
|
||||
return 0.21817*9.81*(-0.091578999999999994*sin(q2)*sin(q3 + 1.5707963267948966) - 0.29910000000000003*sin(q2)*cos(q3 + 1.5707963267948966) - 0.29910000000000003*sin(q3 + 1.5707963267948966)*cos(q2) + 0.091578999999999994*cos(q2)*cos(q3 + 1.5707963267948966));
|
||||
return 0.54317*9.81*(-0.091578999999999994*sin(q2)*sin(q3 + 1.5707963267948966) - 0.29910000000000003*sin(q2)*cos(q3 + 1.5707963267948966) - 0.29910000000000003*sin(q3 + 1.5707963267948966)*cos(q2) + 0.091578999999999994*cos(q2)*cos(q3 + 1.5707963267948966));
|
||||
}
|
||||
double compute_l4q3(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
|
||||
return 0.60215*9.81*(-0.091578999999999994*sin(q2)*sin(q3 + 1.5707963267948966) - 0.189273*sin(q2)*cos(q3 + 1.5707963267948966) - 0.189273*sin(q3 + 1.5707963267948966)*cos(q2) + 0.091578999999999994*cos(q2)*cos(q3 + 1.5707963267948966));
|
||||
return 0.92715*9.81*(-0.091578999999999994*sin(q2)*sin(q3 + 1.5707963267948966) - 0.189273*sin(q2)*cos(q3 + 1.5707963267948966) - 0.189273*sin(q3 + 1.5707963267948966)*cos(q2) + 0.091578999999999994*cos(q2)*cos(q3 + 1.5707963267948966));
|
||||
}
|
||||
double compute_l3q3(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
|
||||
return 0.72964*9.81*(-0.086979000000000001*sin(q2)*sin(q3 + 1.5707963267948966) + 0.086979000000000001*cos(q2)*cos(q3 + 1.5707963267948966));
|
||||
return 1.05464*9.81*(-0.086979000000000001*sin(q2)*sin(q3 + 1.5707963267948966) + 0.086979000000000001*cos(q2)*cos(q3 + 1.5707963267948966));
|
||||
}
|
||||
|
||||
//joint4
|
||||
double compute_l5q4(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
|
||||
return 0.21817*9.81*(-0.058000000000000003*(-(1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*sin(q4) + (-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*cos(q4))*sin(q5) + 0.058000000000000003*(-6.123233995736766e-17*(1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*cos(q4) - 6.123233995736766e-17*(-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*sin(q4))*cos(q5) + 3.5514757175273241e-18*(1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*cos(q4) + 3.5514757175273241e-18*(-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*sin(q4));
|
||||
return 0.54317*9.81*(-0.058000000000000003*(-(1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*sin(q4) + (-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*cos(q4))*sin(q5) + 0.058000000000000003*(-6.123233995736766e-17*(1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*cos(q4) - 6.123233995736766e-17*(-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*sin(q4))*cos(q5) + 3.5514757175273241e-18*(1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*cos(q4) + 3.5514757175273241e-18*(-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*sin(q4));
|
||||
}
|
||||
|
||||
//joint5
|
||||
double compute_l5q5(double q1, double q2, double q3, double q4, double q5,double q6,double q7) {
|
||||
return 0.21817*9.81*(-0.058000000000000003*((1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*cos(q4) + (-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*sin(q4))*cos(q5) - 0.058000000000000003*(-6.123233995736766e-17*(1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*sin(q4) + 6.123233995736766e-17*(-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*cos(q4) - 1.0*sin(q2)*sin(q3 + 1.5707963267948966) + 1.0*cos(q2)*cos(q3 + 1.5707963267948966) + 3.749399456654644e-33)*sin(q5));
|
||||
return 0.54317*9.81*(-0.058000000000000003*((1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*cos(q4) + (-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*sin(q4))*cos(q5) - 0.058000000000000003*(-6.123233995736766e-17*(1.0*sin(q2)*cos(q3 + 1.5707963267948966) + 1.0*sin(q3 + 1.5707963267948966)*cos(q2))*sin(q4) + 6.123233995736766e-17*(-6.123233995736766e-17*sin(q2)*sin(q3 + 1.5707963267948966) + 6.123233995736766e-17*cos(q2)*cos(q3 + 1.5707963267948966) - 6.123233995736766e-17)*cos(q4) - 1.0*sin(q2)*sin(q3 + 1.5707963267948966) + 1.0*cos(q2)*cos(q3 + 1.5707963267948966) + 3.749399456654644e-33)*sin(q5));
|
||||
}
|
||||
|
||||
|
||||
@ -89,7 +90,6 @@ double q2t(double q1, double q2, double q3, double q4, double q5,double q6,doubl
|
||||
+ compute_l3q2(q1, q2, q3, q4, q5, q6, q7)
|
||||
+ compute_l2q2(q1, q2, q3, q4, q5, q6, q7);
|
||||
}
|
||||
|
||||
double q3t(double q1, double q2, double q3, double q4, double q5,double q6,double q7)
|
||||
{
|
||||
return \
|
||||
@ -99,8 +99,6 @@ double q3t(double q1, double q2, double q3, double q4, double q5,double q6,doubl
|
||||
// +compute_l6q3(q1, q2, q3, q4, q5, q6-pi/2, q7);
|
||||
// +compute_l7q3(q1, q2, q3, q4, q5, q6-pi/2, q7);
|
||||
}
|
||||
|
||||
|
||||
double q4t(double q1, double q2, double q3, double q4, double q5,double q6,double q7)
|
||||
{
|
||||
return \
|
||||
@ -140,8 +138,14 @@ void arm_init(arm_t* arm,Config_RobotParam_t *cfg)
|
||||
|
||||
arm->mass[i]=cfg->arm_param.mass[i];
|
||||
}
|
||||
arm_path_start(arm, (float[5]){1,1,0,0,0});
|
||||
|
||||
}
|
||||
|
||||
float j1b=0;
|
||||
float j2b=0;
|
||||
float j3b=0;
|
||||
float j4b=0;
|
||||
//机械臂重力补偿计算
|
||||
void arm_gravity_calcu(arm_t* arm)
|
||||
{
|
||||
@ -152,8 +156,12 @@ void arm_gravity_calcu(arm_t* arm)
|
||||
arm->joint[3].feedback.position,0,0);
|
||||
arm->joint[1].ref.torque += (float)q3t(0 ,arm->joint[0].feedback.position,arm->joint[1].feedback.position,arm->joint[2].feedback.position,
|
||||
arm->joint[3].feedback.position,0,0);
|
||||
arm->joint[0].ref.torque += (float)q2t(0 ,arm->joint[0].feedback.position,arm->joint[1].feedback.position,arm->joint[2].feedback.position,
|
||||
arm->joint[0].ref.torque += 0.9*(float)q2t(0 ,arm->joint[0].feedback.position,arm->joint[1].feedback.position,arm->joint[2].feedback.position,
|
||||
arm->joint[3].feedback.position,0,0);
|
||||
j1b=arm->joint[0].ref.torque;
|
||||
j2b=arm->joint[1].ref.torque;
|
||||
j3b=arm->joint[2].ref.torque;
|
||||
j4b=arm->joint[3].ref.torque;
|
||||
}
|
||||
|
||||
|
||||
@ -194,8 +202,9 @@ void arm_pid_calcu(arm_t* arm)
|
||||
assert_param(!(arm==NULL));
|
||||
for (uint8_t i=0; i<JOINT_NUM; i++) {
|
||||
float now_time = DWT_GetTimeline_s();
|
||||
arm->joint[i].ref.velocity+= PID_Calc(&arm->joint[i].controler.Ppid, arm->joint[i].ref.position, arm->joint[i].feedback.position,arm->joint[i].feedback.velocity,now_time-arm->joint[i].status.last_control_time);
|
||||
arm->joint[i].ref.torque += PID_Calc(&arm->joint[i].controler.Vpid, arm->joint[i].ref.velocity, arm->joint[i].feedback.velocity, 0, now_time-arm->joint[i].status.last_control_time);
|
||||
float dt = now_time - arm->joint[i].status.last_control_time;
|
||||
arm->joint[i].ref.velocity+= PID_Calc(&arm->joint[i].controler.Ppid, arm->joint[i].ref.position, arm->joint[i].feedback.position,arm->joint[i].feedback.velocity,0.001);
|
||||
arm->joint[i].ref.torque += PID_Calc(&arm->joint[i].controler.Vpid, arm->joint[i].ref.velocity, arm->joint[i].feedback.velocity, 0, 0.001);
|
||||
arm->joint[i].status.last_control_time = now_time;
|
||||
}
|
||||
}
|
||||
@ -204,20 +213,31 @@ void arm_pid_calcu(arm_t* arm)
|
||||
void arm_control_apply(arm_t* arm)
|
||||
{
|
||||
assert_param(!(arm==NULL));
|
||||
|
||||
|
||||
for (uint8_t i=0; i<JOINT_NUM; i++) {
|
||||
assert_param(!(arm->joint[i].motor.handle == NULL));
|
||||
|
||||
arm->joint[i].ref.torque =0;
|
||||
|
||||
if(arm->joint[i].feedback.position<arm->joint[i].status.angle_limit[0]||arm->joint[i].feedback.position>arm->joint[i].status.angle_limit[1])
|
||||
{
|
||||
arm->joint[i].ref.torque=0;
|
||||
}
|
||||
}
|
||||
arm->joint[0].ref.torque -= arm->joint[1].ref.torque;
|
||||
for (uint8_t i=0; i<JOINT_NUM; i++) {
|
||||
assert_param(!(arm->joint[i].motor.handle == NULL));
|
||||
|
||||
|
||||
|
||||
// arm->joint[i].ref.torque=0;
|
||||
|
||||
|
||||
|
||||
|
||||
// arm->joint[i].ref.torque = _constrain(arm->joint[i].ref.torque,-1,1);
|
||||
switch (arm->joint[i].motor.type) {
|
||||
case DM_MOTOR:
|
||||
{
|
||||
MOTOR_DM_t* handle= __dm_handle(arm->joint[i].motor.handle);
|
||||
MOTOR_MIT_Output_t out;
|
||||
MOTOR_MIT_Output_t out={0};
|
||||
out.torque = arm->joint[i].ref.torque;
|
||||
MOTOR_DM_MITCtrl(&handle->param, &out);
|
||||
break;
|
||||
@ -225,14 +245,13 @@ arm->joint[i].ref.torque =0;
|
||||
case LZ_MOTOR:
|
||||
{
|
||||
MOTOR_LZ_t* handle= __lz_handle(arm->joint[i].motor.handle);
|
||||
MOTOR_LZ_MotionParam_t out;
|
||||
MOTOR_LZ_MotionParam_t out={0};
|
||||
out.torque = arm->joint[i].ref.torque;
|
||||
MOTOR_LZ_MotionControl(&handle->param, &out);
|
||||
break;
|
||||
}
|
||||
}
|
||||
arm->joint[i].ref.torque=0;
|
||||
arm->joint[i].ref.position=0;
|
||||
arm->joint[i].ref.velocity=0;
|
||||
}
|
||||
}
|
||||
@ -266,6 +285,8 @@ void arm_relax(arm_t* arm)
|
||||
void arm_path_start(arm_t* arm,float* target_angle)
|
||||
{
|
||||
assert_param(!(arm==NULL));
|
||||
|
||||
|
||||
float time=0;
|
||||
float delta[JOINT_NUM];
|
||||
float max_delta=0;
|
||||
@ -278,18 +299,19 @@ void arm_path_start(arm_t* arm,float* target_angle)
|
||||
time = max_delta/10.0f;
|
||||
|
||||
for (uint8_t i=0; i<JOINT_NUM; i++) {
|
||||
PathStart(&arm->joint[i].controler.path,arm->joint[i].feedback.position,target_angle[i],0,0,time,DWT_GetTimeline_s());
|
||||
PathStart(&arm->joint[i].controler.path,arm->joint[i].feedback.position,target_angle[i],0,0,3000,HAL_GetTick());
|
||||
}
|
||||
}
|
||||
|
||||
void arm_path_process(arm_t* arm)
|
||||
{
|
||||
assert_param(!(arm==NULL));
|
||||
|
||||
int finish_num=0;
|
||||
for(int i=0;i<JOINT_NUM;i++)
|
||||
{
|
||||
PathCalcu(&arm->joint[i].controler.path, DWT_GetTimeline_s());
|
||||
arm->joint[i].ref.position += arm->joint[i].controler.path.pout; //当前路径位置
|
||||
PathCalcu(&arm->joint[i].controler.path,HAL_GetTick());
|
||||
arm->joint[i].ref.position = arm->joint[i].controler.path.pout; //当前路径位置
|
||||
arm->joint[i].ref.velocity += arm->joint[i].controler.path.vout; //当前路径速度前馈
|
||||
|
||||
finish_num+= arm->joint[i].controler.path.state;
|
||||
@ -332,7 +354,7 @@ void arm_follow_control(arm_t* arm,CMD_Arm_t* data)
|
||||
{
|
||||
arm->status.busy=0; //退出忙碌，可以接收指令
|
||||
for (int i=0; i<JOINT_NUM; i++) {
|
||||
arm->joint[i].ref.position += data->data.ref_angle[i]; //跟踪控制器
|
||||
arm->joint[i].ref.position = data->data.ref_angle[i]; //跟踪控制器
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
@ -115,3 +115,6 @@ void arm_init(arm_t* arm,Config_RobotParam_t *cfg);
|
||||
void arm_data_updata(arm_t* arm);
|
||||
void arm_control_apply(arm_t* arm);
|
||||
void arm_gravity_calcu(arm_t* arm);
|
||||
void arm_pid_calcu(arm_t* arm);
|
||||
void arm_path_start(arm_t* arm,float* target_angle);
|
||||
void arm_path_process(arm_t* arm);
|
||||
|
||||
@ -7,12 +7,40 @@
|
||||
static Config_RobotParam_t robot_config = {
|
||||
.arm_param = {
|
||||
.joint[0]={
|
||||
.angle_limit={0,3.0},
|
||||
.angle_limit={-1,3.0},
|
||||
.init_angle = -1.384,
|
||||
.motor.type = LZ_MOTOR,
|
||||
.motor.param.lz_param={
|
||||
.motor_id=2,.host_id=2,.can=BSP_CAN_1,.mode=MOTOR_LZ_MODE_MOTION,.module=MOTOR_LZ_RSO3,.reverse=1
|
||||
},
|
||||
.pid.Ppid={
|
||||
.k=0.1,
|
||||
.p=1,
|
||||
.i=0,
|
||||
.d=0,
|
||||
.i_limit=0,
|
||||
.out_limit=100,
|
||||
.d_cutoff_freq=100
|
||||
},
|
||||
.pid.Vpid={
|
||||
.k=1,
|
||||
.p=1,
|
||||
.i=0,
|
||||
.d=0,
|
||||
.i_limit=10,
|
||||
.out_limit=20,
|
||||
.d_cutoff_freq=100
|
||||
}
|
||||
|
||||
|
||||
},
|
||||
.joint[1]={
|
||||
.angle_limit={-6,1},
|
||||
.init_angle = 1.538,
|
||||
.motor.type = LZ_MOTOR,
|
||||
.motor.param.lz_param={
|
||||
.motor_id=3,.host_id=3,.can=BSP_CAN_1,.mode=MOTOR_LZ_MODE_MOTION,.module=MOTOR_LZ_RSO3,.reverse=0
|
||||
},
|
||||
.pid.Ppid={
|
||||
.k=1,
|
||||
.p=0,
|
||||
@ -21,18 +49,10 @@ static Config_RobotParam_t robot_config = {
|
||||
.i_limit=10,
|
||||
.out_limit=20,
|
||||
.d_cutoff_freq=100
|
||||
}
|
||||
},
|
||||
.joint[1]={
|
||||
.angle_limit={-6,0},
|
||||
.init_angle = 1.668,
|
||||
.motor.type = LZ_MOTOR,
|
||||
.motor.param.lz_param={
|
||||
.motor_id=3,.host_id=3,.can=BSP_CAN_1,.mode=MOTOR_LZ_MODE_MOTION,.module=MOTOR_LZ_RSO3,.reverse=0
|
||||
},
|
||||
.pid.Ppid={
|
||||
.pid.Vpid={
|
||||
.k=1,
|
||||
.p=0,
|
||||
.p=1,
|
||||
.i=0,
|
||||
.d=0,
|
||||
.i_limit=10,
|
||||
@ -55,6 +75,15 @@ static Config_RobotParam_t robot_config = {
|
||||
.i_limit=10,
|
||||
.out_limit=20,
|
||||
.d_cutoff_freq=100
|
||||
},
|
||||
.pid.Vpid={
|
||||
.k=1,
|
||||
.p=0.5,
|
||||
.i=0,
|
||||
.d=0,
|
||||
.i_limit=10,
|
||||
.out_limit=20,
|
||||
.d_cutoff_freq=100
|
||||
}
|
||||
},
|
||||
|
||||
@ -71,6 +100,15 @@ static Config_RobotParam_t robot_config = {
|
||||
.i=0,
|
||||
.d=0,
|
||||
.i_limit=10,
|
||||
.out_limit=10,
|
||||
.d_cutoff_freq=100
|
||||
},
|
||||
.pid.Vpid={
|
||||
.k=1,
|
||||
.p=1,
|
||||
.i=0,
|
||||
.d=0,
|
||||
.i_limit=10,
|
||||
.out_limit=20,
|
||||
.d_cutoff_freq=100
|
||||
}
|
||||
@ -90,6 +128,15 @@ static Config_RobotParam_t robot_config = {
|
||||
.i_limit=10,
|
||||
.out_limit=20,
|
||||
.d_cutoff_freq=100
|
||||
},
|
||||
.pid.Vpid={
|
||||
.k=1,
|
||||
.p=1,
|
||||
.i=0,
|
||||
.d=0,
|
||||
.i_limit=10,
|
||||
.out_limit=20,
|
||||
.d_cutoff_freq=100
|
||||
}
|
||||
},
|
||||
}
|
||||
|
||||
@ -99,9 +99,32 @@ MOTOR_LZ_t* a;
|
||||
|
||||
|
||||
|
||||
float ref_p[5]={0,0,0,0,0};
|
||||
|
||||
|
||||
float refpid=0;
|
||||
float refp=0;
|
||||
float refp1=0;
|
||||
arm_t arm;
|
||||
|
||||
|
||||
float key_status=0;
|
||||
void key_scan()
|
||||
{
|
||||
if(HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_0)==0)
|
||||
{
|
||||
key_status=1;
|
||||
}
|
||||
|
||||
if(HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_0)==1&&key_status==1)
|
||||
{
|
||||
arm_path_start(&arm,ref_p);
|
||||
key_status=0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -129,12 +152,22 @@ void Task_control(void *argument) {
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
//
|
||||
// arm.joint[0].ref.position = refp;
|
||||
// arm.joint[1].ref.position = refp1;
|
||||
// arm.joint[0].ref.torque = reft;
|
||||
|
||||
key_scan();
|
||||
arm_path_process(&arm);
|
||||
arm_pid_calcu(&arm);
|
||||
refpid = arm.joint[0].ref.torque;
|
||||
arm_gravity_calcu(&arm);
|
||||
arm_control_apply(&arm);
|
||||
|
||||
arm_data_updata(&arm);
|
||||
|
||||
|
||||
|
||||
// MOTOR_LZ_UpdateAll();
|
||||
// MOTOR_LZ_Update(&lzmotor.param);
|
||||
// MOTOR_LZ_MotionControl(&lzmotor.param, &control);
|
||||
|
||||
363
engineer/lklk.jdebug
Normal file
363
engineer/lklk.jdebug
Normal file
@ -0,0 +1,363 @@
|
||||
/*********************************************************************
|
||||
* (c) SEGGER Microcontroller GmbH *
|
||||
* The Embedded Experts *
|
||||
* www.segger.com *
|
||||
**********************************************************************
|
||||
|
||||
File :
|
||||
Created : 28. Dec 2025 11:30
|
||||
Ozone Version : V3.40d
|
||||
*/
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnProjectLoad
|
||||
*
|
||||
* Function description
|
||||
* Project load routine. Required.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
void OnProjectLoad (void) {
|
||||
//
|
||||
// Dialog-generated settings
|
||||
//
|
||||
Project.AddPathSubstitute ("/home/shentou/workspace/engineer/engineer", "$(ProjectDir)");
|
||||
Project.AddPathSubstitute ("/home/shentou/workspace/engineer/engineer", "$(ProjectDir)");
|
||||
Project.SetDevice ("STM32F407IG");
|
||||
Project.SetHostIF ("USB", "");
|
||||
Project.SetTargetIF ("SWD");
|
||||
Project.SetTIFSpeed ("4 MHz");
|
||||
Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M4F.svd");
|
||||
Project.AddSvdFile ("$(InstallDir)/Config/Peripherals/STM32F407IG.svd");
|
||||
//
|
||||
// User settings
|
||||
//
|
||||
Project.SetOSPlugin ("FreeRTOSPlugin_Cortex-M");
|
||||
File.Open ("$(ProjectDir)/build/Debug/engineer.elf");
|
||||
}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnStartupComplete
|
||||
*
|
||||
* Function description
|
||||
* Called when program execution has reached/passed
|
||||
* the startup completion point. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnStartupComplete (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* TargetReset
|
||||
*
|
||||
* Function description
|
||||
* Replaces the default target device reset routine. Optional.
|
||||
*
|
||||
* Notes
|
||||
* This example demonstrates the usage when
|
||||
* debugging an application in RAM on a Cortex-M target device.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void TargetReset (void) {
|
||||
//
|
||||
// unsigned int SP;
|
||||
// unsigned int PC;
|
||||
// unsigned int VectorTableAddr;
|
||||
//
|
||||
// VectorTableAddr = Elf.GetBaseAddr();
|
||||
// //
|
||||
// // Set up initial stack pointer
|
||||
// //
|
||||
// if (VectorTableAddr != 0xFFFFFFFF) {
|
||||
// SP = Target.ReadU32(VectorTableAddr);
|
||||
// Target.SetReg("SP", SP);
|
||||
// }
|
||||
// //
|
||||
// // Set up entry point PC
|
||||
// //
|
||||
// PC = Elf.GetEntryPointPC();
|
||||
//
|
||||
// if (PC != 0xFFFFFFFF) {
|
||||
// Target.SetReg("PC", PC);
|
||||
// } else if (VectorTableAddr != 0xFFFFFFFF) {
|
||||
// PC = Target.ReadU32(VectorTableAddr + 4);
|
||||
// Target.SetReg("PC", PC);
|
||||
// } else {
|
||||
// Util.Error("Project file error: failed to set entry point PC", 1);
|
||||
// }
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetReset
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetReset (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetReset
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
* The default implementation initializes SP and PC to reset values.
|
||||
**
|
||||
**********************************************************************
|
||||
*/
|
||||
void AfterTargetReset (void) {
|
||||
_SetupTarget();
|
||||
}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* DebugStart
|
||||
*
|
||||
* Function description
|
||||
* Replaces the default debug session startup routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void DebugStart (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* TargetConnect
|
||||
*
|
||||
* Function description
|
||||
* Replaces the default target IF connection routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void TargetConnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetConnect
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetConnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetConnect
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void AfterTargetConnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* TargetDownload
|
||||
*
|
||||
* Function description
|
||||
* Replaces the default program download routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void TargetDownload (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetDownload
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetDownload (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetDownload
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
* The default implementation initializes SP and PC to reset values.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
void AfterTargetDownload (void) {
|
||||
_SetupTarget();
|
||||
}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetDisconnect
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetDisconnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetDisconnect
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void AfterTargetDisconnect (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterTargetHalt
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void AfterTargetHalt (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* BeforeTargetResume
|
||||
*
|
||||
* Function description
|
||||
* Event handler routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void BeforeTargetResume (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnSnapshotLoad
|
||||
*
|
||||
* Function description
|
||||
* Called upon loading a snapshot. Optional.
|
||||
*
|
||||
* Additional information
|
||||
* This function is used to restore the target state in cases
|
||||
* where values cannot simply be written to the target.
|
||||
* Typical use: GPIO clock needs to be enabled, before
|
||||
* GPIO is configured.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnSnapshotLoad (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnSnapshotSave
|
||||
*
|
||||
* Function description
|
||||
* Called upon saving a snapshot. Optional.
|
||||
*
|
||||
* Additional information
|
||||
* This function is usually used to save values of the target
|
||||
* state which can either not be trivially read,
|
||||
* or need to be restored in a specific way or order.
|
||||
* Typically use: Memory Mapped Registers,
|
||||
* such as PLL and GPIO configuration.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnSnapshotSave (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnError
|
||||
*
|
||||
* Function description
|
||||
* Called when an error ocurred. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnError (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* AfterProjectLoad
|
||||
*
|
||||
* Function description
|
||||
* After Project load routine. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void AfterProjectLoad (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* OnDebugStartBreakSymbolReached
|
||||
*
|
||||
* Function description
|
||||
* Called when program execution has reached/passed
|
||||
* the symbol to be breaked at during debug start. Optional.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
//void OnDebugStartBreakSymReached (void) {
|
||||
//}
|
||||
|
||||
/*********************************************************************
|
||||
*
|
||||
* _SetupTarget
|
||||
*
|
||||
* Function description
|
||||
* Setup the target.
|
||||
* Called by AfterTargetReset() and AfterTargetDownload().
|
||||
*
|
||||
* Auto-generated function. May be overridden by Ozone.
|
||||
*
|
||||
**********************************************************************
|
||||
*/
|
||||
void _SetupTarget(void) {
|
||||
unsigned int SP;
|
||||
unsigned int PC;
|
||||
unsigned int VectorTableAddr;
|
||||
|
||||
VectorTableAddr = Elf.GetBaseAddr();
|
||||
//
|
||||
// Set up initial stack pointer
|
||||
//
|
||||
SP = Target.ReadU32(VectorTableAddr);
|
||||
if (SP != 0xFFFFFFFF) {
|
||||
Target.SetReg("SP", SP);
|
||||
}
|
||||
//
|
||||
// Set up entry point PC
|
||||
//
|
||||
PC = Elf.GetEntryPointPC();
|
||||
if (PC != 0xFFFFFFFF) {
|
||||
Target.SetReg("PC", PC);
|
||||
} else {
|
||||
Util.Error("Project script error: failed to set up entry point PC", 1);
|
||||
}
|
||||
}
|
||||
43
engineer/lklk.jdebug.user
Normal file
43
engineer/lklk.jdebug.user
Normal file
@ -0,0 +1,43 @@
|
||||
|
||||
|
||||
GraphedExpression="(((arm).joint[2]).feedback).position", Color=#e56a6f, Show=0
|
||||
GraphedExpression="(((arm).joint[0]).feedback).position", Color=#35792b, Show=0
|
||||
GraphedExpression="(((arm).joint[4]).feedback).position", Color=#769dda, Show=0
|
||||
GraphedExpression="(((arm).joint[3]).feedback).position", Color=#b14f0d, Show=0
|
||||
GraphedExpression="(((arm).joint[1]).feedback).position", Color=#b3c38e, Show=0
|
||||
GraphedExpression="j1b", Color=#ab7b05, Show=0
|
||||
GraphedExpression="j2b", Color=#7fd3b7, Show=0
|
||||
GraphedExpression="j3b", Color=#50328f, Show=0
|
||||
GraphedExpression="j4b", Color=#c587a5, Show=0
|
||||
GraphedExpression="refpid", Color=#999401, Show=0
|
||||
OpenDocument="ieee754-df.S", FilePath="/build/reproducible-path/gcc-arm-none-eabi-14.2.rel1/libgcc/config/arm/ieee754-df.S", Line=0
|
||||
OpenDocument="startup_stm32f407xx.s", FilePath="/home/shentou/workspace/engineer/engineer/startup_stm32f407xx.s", Line=48
|
||||
OpenDocument="config.h", FilePath="/home/shentou/workspace/engineer/engineer/User/module/config.h", Line=0
|
||||
OpenDocument="pid.c", FilePath="/home/shentou/workspace/engineer/engineer/User/component/pid.c", Line=76
|
||||
OpenDocument="config.c", FilePath="/home/shentou/workspace/engineer/engineer/User/module/config.c", Line=13
|
||||
OpenDocument="motor_lz.c", FilePath="/home/shentou/workspace/engineer/engineer/User/device/motor_lz.c", Line=178
|
||||
OpenDocument="main.c", FilePath="/home/shentou/workspace/engineer/engineer/Core/Src/main.c", Line=55
|
||||
OpenDocument="can.c", FilePath="/home/shentou/workspace/engineer/engineer/User/bsp/can.c", Line=498
|
||||
OpenDocument="motor_dm.c", FilePath="/home/shentou/workspace/engineer/engineer/User/device/motor_dm.c", Line=319
|
||||
OpenDocument="arm.c", FilePath="/home/shentou/workspace/engineer/engineer/User/module/arm.c", Line=281
|
||||
OpenDocument="control.c", FilePath="/home/shentou/workspace/engineer/engineer/User/task/control.c", Line=141
|
||||
OpenToolbar="Debug", Floating=0, x=0, y=0
|
||||
OpenWindow="Call Stack", DockArea=RIGHT, x=0, y=1, w=813, h=576, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=300, h=963, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=813, h=386, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Watched Data 1", DockArea=BOTTOM, x=0, y=0, w=868, h=332, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Data Sampling", DockArea=BOTTOM, x=1, y=0, w=1691, h=332, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
|
||||
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
|
||||
TableHeader="Call Stack", SortCol="Function", SortOrder="ASCENDING", VisibleCols=["Function";"Stack Frame";"Source";"PC";"Return Address";"Stack Used"], ColWidths=[105;100;189;100;100;219]
|
||||
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[204;100;100;100;819]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (((arm).joint[2]).feedback).position";" (((arm).joint[0]).feedback).position";" (((arm).joint[4]).feedback).position";" (((arm).joint[3]).feedback).position";" (((arm).joint[1]).feedback).position";" j1b";" j2b";" j3b";" j4b";" refpid"], ColWidths=[100;100;100;100;100;100;100;100;100;100;100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[262;100;100;100;100;100;100;104;725]
|
||||
TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
|
||||
TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[262;100;100;390]
|
||||
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
|
||||
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;359]
|
||||
WatchedExpression="arm", Window=Watched Data 1
|
||||
WatchedExpression="refp", Window=Watched Data 1
|
||||
WatchedExpression="refp1", Window=Watched Data 1
|
||||
WatchedExpression="ref_p", Window=Watched Data 1
|
||||
Loading…
Reference in New Issue
Block a user