【RTOS】TARM端水机械臂控制卡
本帖最后由 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如果定义 #define FINSH_USING_MSH_ONLY 还能直接进入msh模式，否则会进入C style，要键入msh()才能进入msh模式。此外还能通过定义：
#define FINSH_USING_HISTORY 0
#define FINSH_HISTORY_LINES 1使能历史指令
2-自定义三个个msh指令：TARM_MOVXYTARM_MOVRTARM_CloopLoop分别进行垂直面的驱动与回转面的驱动，置位作为标志位的全局变量，开始机械臂运动
#ifdef FINSH_USING_MSH
void TARM_MOVXY(int argc, char**argv)
{
double x=0,y=0;
int speed=0;
char t_str;
if(argc <= 2 || argc >= 5)
{
rt_kprintf("too little or too many arg!");
return ;
}
x = atof(argv);
y = atof(argv);
if(argc==3)
{
speed=100;
}
else if(argc==4)
{
speed=atof(argv);
}
// 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;
if(argc <= 1 || argc >= 4)
{
rt_kprintf("too little or too many arg!");
return ;
}
r = atof(argv);
if(argc==2)
{
speed=100;
}
else if(argc==3)
{
speed=atof(argv);
}
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;
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;
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指令下发演示：
文档：
页:
[1]