diff --git a/engineer/CMakeLists.txt b/engineer/CMakeLists.txt index 37e6348..a0a51ed 100644 --- a/engineer/CMakeLists.txt +++ b/engineer/CMakeLists.txt @@ -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 diff --git a/engineer/Core/Src/gpio.c b/engineer/Core/Src/gpio.c index e80413f..c56ff41 100644 --- a/engineer/Core/Src/gpio.c +++ b/engineer/Core/Src/gpio.c @@ -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 */ diff --git a/engineer/DM.ioc b/engineer/DM.ioc index 28781a0..487b0e6 100644 --- a/engineer/DM.ioc +++ b/engineer/DM.ioc @@ -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 diff --git a/engineer/User/module/arm.c b/engineer/User/module/arm.c index d98d334..0b162df 100644 --- a/engineer/User/module/arm.c +++ b/engineer/User/module/arm.c @@ -9,6 +9,7 @@ #include "module/config.h" +#define _constrain(in,min,max) (inmax)?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; ijoint[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; ijoint[i].motor.handle == NULL)); - -arm->joint[i].ref.torque =0; - if(arm->joint[i].feedback.positionjoint[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; ijoint[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; ijoint[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;ijoint[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; ijoint[i].ref.position += data->data.ref_angle[i]; //跟踪控制器 + arm->joint[i].ref.position = data->data.ref_angle[i]; //跟踪控制器 } break; } diff --git a/engineer/User/module/arm.h b/engineer/User/module/arm.h index bcc2d88..0d1c589 100644 --- a/engineer/User/module/arm.h +++ b/engineer/User/module/arm.h @@ -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); diff --git a/engineer/User/module/config.c b/engineer/User/module/config.c index 3fe5e11..3247e94 100644 --- a/engineer/User/module/config.c +++ b/engineer/User/module/config.c @@ -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 } }, } diff --git a/engineer/User/task/control.c b/engineer/User/task/control.c index fd1224a..d9699ca 100644 --- a/engineer/User/task/control.c +++ b/engineer/User/task/control.c @@ -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); diff --git a/engineer/lklk.jdebug b/engineer/lklk.jdebug new file mode 100644 index 0000000..c56baa5 --- /dev/null +++ b/engineer/lklk.jdebug @@ -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); + } +} \ No newline at end of file diff --git a/engineer/lklk.jdebug.user b/engineer/lklk.jdebug.user new file mode 100644 index 0000000..9358b93 --- /dev/null +++ b/engineer/lklk.jdebug.user @@ -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 \ No newline at end of file