本帖最后由 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);
}
}
四、程序源码
五、视频演示 机械臂驱动演示：https://v.youku.com/v_show/id_XNDczOTU2NDkyOA==.html 指令下发演示：
| 
楼主
标题置顶
标题高亮
