本帖最后由 tinnu 于 2020-7-13 20:01 编辑
一、概述 该参赛作品为针对液体搬运环境优化的机械臂下位机运动控制卡。针对日常生活中一个常见的场景——搬运液体(俗称“端水”),设计一项高效平稳的控制算法,配合自主开发的控制器驱动板卡,驱动自制的机械臂到达目标位置,使该算法达到高效平稳搬运液体的目的。
二、硬件方案 1-驱动方案 步进电机驱动:A4988+DRV8825 供电体系设计:12V DC输入,MP1584两路 角度反馈:两个MPU6500,通过IIC接口通讯
2-驱动板 驱动板为兼容RAMPS1.4设计:
3-控制板 控制板采用新塘的M451 64脚芯片作为主控,这款芯片位M4F架构,能充分发挥浮点运算能力与DSP运算能力,主频高达72MHz。 原理图、PCB工程:https://oshwhub.com/day_day/tarm_v1-4
三、软件方案 1-使能msh控制台 在rtconfig.h里面增加定义: 如果定义 #define FINSH_USING_MSH_ONLY 还能直接进入msh模式,否则会进入C style,要键入msh()才能进入msh模式。
此外还能通过定义:
- #define FINSH_USING_HISTORY 0
- #define FINSH_HISTORY_LINES 1
使能历史指令
2-自定义三个个msh指令: TARM_MOVXY TARM_MOVR TARM_CloopLoop 分别进行垂直面的驱动与回转面的驱动,置位作为标志位的全局变量,开始机械臂运动
- #ifdef FINSH_USING_MSH
- void TARM_MOVXY(int argc, char**argv)
- {
- double x=0,y=0;
- int speed=0;
- char t_str[30];
- if(argc <= 2 || argc >= 5)
- {
- rt_kprintf("too little or too many arg!");
- return ;
- }
- x = atof(argv[1]);
- y = atof(argv[2]);
- if(argc==3)
- {
- speed=100;
- }
- else if(argc==4)
- {
- speed=atof(argv[3]);
- }
- // rt_kprintf("x-%f,y-%f,s-%f",x,y,speed);
- g_TARM_RunRecord.ReadyRunLoc_x = x;
- g_TARM_RunRecord.ReadyRunLoc_y = y;
- g_TARM_RunRecord.ReadyRunLoc_speed = speed;
- g_TARM_RunRecord.StartRun_pv =1;
- sprintf(t_str,"_&x:%f;\t_&y:%f;\n_&speed:%d;\r\r\n",x,y,speed);
- rt_kprintf(t_str);
- }
- MSH_CMD_EXPORT(TARM_MOVXY, mov with X Y speed : TARM_MOVXY 7.1 12.1 150.1);
- void TARM_MOVR(int argc, char**argv)
- {
- double r=0;
- int speed=0;
- char t_str[30];
- if(argc <= 1 || argc >= 4)
- {
- rt_kprintf("too little or too many arg!");
- return ;
- }
- r = atof(argv[1]);
- if(argc==2)
- {
- speed=100;
- }
- else if(argc==3)
- {
- speed=atof(argv[2]);
- }
- g_TARM_RunRecord.ReadyRunLoc_r = r;
- g_TARM_RunRecord.ReadyRunLoc_speedr = speed;
- g_TARM_RunRecord.StartRun_r =1;
- sprintf(t_str,"_&r:%f;\n_&speed:%d;\r\r\n",r,speed);
- rt_kprintf(t_str);
- }
- MSH_CMD_EXPORT(TARM_MOVR, mov with X Y speed : TARM_MOVR 100 150.1);
- #endif
TARM_CloopLoop:
- #ifdef FINSH_USING_MSH
- void TARM_CloopLoop()
- {
- rt_thread_resume(clossloop_update_thread);
- }
- MSH_CMD_EXPORT(TARM_CloopLoop, closeloop for once : TARM_CloopLoop);
- #endif
3-设计一个线程进行机械臂驱动 动驱动机械臂的循环操作函数:handle_pvMainLoop、handle_rMainLoop 创建一个线程,通过识别标志位判断是否驱动机械臂。 - static void TARM_DriverStepSteering(void *parameter)
- {
- char t_strReport[30];
- set_current_start();
- while (1)
- {
- if (g_TARM_RunRecord.StartRun_pv == 0 && g_TARM_RunRecord.StartRun_r == 0)
- {
- //让出CPU
- rt_thread_mdelay(500);
- // rt_thread_suspend(rt_thread_self());
- // rt_schedule();
- }
- else if (g_TARM_RunRecord.StartRun_pv != 0)
- {
- //驱动
- handle_pvMainLoop(g_TARM_RunRecord.ReadyRunLoc_x,
- g_TARM_RunRecord.ReadyRunLoc_y,
- g_TARM_RunRecord.ReadyRunLoc_speed);
- g_TARM_RunRecord.StartRun_pv = 0;
- sprintf(t_strReport, "->pv arrive:\nx:%f\ty:%f\n",
- g_TARM_RunRecord.pv_current.x,
- g_TARM_RunRecord.pv_current.y);
- rt_kprintf(t_strReport);<blockquote>rt_thread_mdelay(300);
4-设计一个线程进行反馈检测 - void clossloop_update()
- {
- char t_strReport[100];
- CloseLoop_Init();
- while(1)
- {
- rt_thread_suspend(rt_thread_self());
- rt_schedule();
- CloseLoop_Gather();
- sprintf(t_strReport, "\n->closeloop result:\n\tpv arrive:\n\tx:%f\ty:%f\n",
- g_TARM_RunRecord.pv_current.x,
- g_TARM_RunRecord.pv_current.y);
- rt_kprintf(t_strReport);
- }
- }
反馈检测一次会挂起,当被唤醒时更新一次数值。
四、程序源码
KEIL工程:
五、视频演示 机械臂驱动演示:https://v.youku.com/v_show/id_XNDczOTU2NDkyOA==.html 指令下发演示:
文档: |