【開源獲獎案例】四軸機械臂控制系統

——來自迪文開發者論壇
本期為大家推送迪文開發者論壇獲獎開源案例——四軸機械臂控制系統。工程師采用T5L智能屏，基于DGUS軟件“旋轉指示”控件實現機械臂的實時位置顯示，并通過串口控制機械臂的運動過程。不同以往只展示機械臂的角度數值，該方案可以實現在屏幕上實時顯示機械臂的旋轉狀態。除此之外，工程師還增加了機械臂自動運動功能。

UI素材展示 UI素材可根據實際的應用進行多樣化的設計，工程師設計了一套富有科技感的界面。

UI開發示例 C51代碼設計

（1）獲取機械臂當前控制的角度：

//獲取機械臂狀態
void get_arm_config_status(){ u8 i = 0; for(i = 0;i < 4;i++) ???{ ???????arm_angle_value[i] = Read_Dgus(0x1100 + i * 2); ???????//Write_Dgus(0x1110 + i * 2, Va[i]); ???}}

（2）機械臂的運動過程設計：使機械臂根據設置的角度，緩慢移動到指定位置。

//設置機械臂角度
void set_arm_angle(){ int i = 0; for(i = 0;i < 4;i++) ?

{ if(arm_angle_value[i] != arm_angle_value_last[i])

{ if(arm_angle_value[i] < arm_angle_value_last[i]) ?????????

{ arm_angle_value_last[i]--; }

else

{ arm_angle_value_last[i]++; }

Write_Dgus(0x1110 + i * 2, arm_angle_value_last[i]); if(i == 1) //第二軸運動

{ u16 armiii_pos_x = 0; u16 armiii_pos_y = 0; u16 armiv_pos_x = 0; u16 armiv_pos_y = 0; armiii_pos_x = sin((float)(225 - arm_angle_value_last[i]) * TRIGONOMETRIC) * 80 + armii_start_pos[0]; armiii_pos_y = cos((float)(225 - arm_angle_value_last[i]) * TRIGONOMETRIC) * 80 + armii_start_pos[1]; armiv_pos_x = sin((float)(180 - arm_angle_value_last[i+1]) * TRIGONOMETRIC) * 83 + armiii_pos_x; armiv_pos_y = cos((float)(180 - arm_angle_value_last[i+1]) * TRIGONOMETRIC) * 83 + armiii_pos_y; armiii_current_pos[0] = armiii_pos_x; armiii_current_pos[1] = armiii_pos_y; OneSendData4(armiii_pos_x/256); OneSendData4(armiii_pos_x%256); OneSendData4(armiii_pos_y/256); OneSendData4(armiii_pos_y%256); Write_Dgus(0x2100 + 4, armiii_pos_x); //第三軸的位置 Write_Dgus(0x2100 + 5, armiii_pos_y); Write_Dgus(0x2200 + 4, armiv_pos_x); //第三軸的位置 Write_Dgus(0x2200 + 5, armiv_pos_y); }

else if(i == 2)

{ u16 armiv_pos_x = sin((float)(180 - arm_angle_value_last[i]) * TRIGONOMETRIC) * 83 + armiii_current_pos[0]; u16 armiv_pos_y = cos((float)(180 - arm_angle_value_last[i]) * TRIGONOMETRIC) * 83 + armiii_current_pos[1]; Write_Dgus(0x2200 + 4, armiv_pos_x); //第三軸的位置 Write_Dgus(0x2200 + 5, armiv_pos_y);

} break;

} }}（3）按鍵設計：開發者設計了復位、添加、刪除、開始、停止按鍵功能//獲取按鍵狀態
void get_key_status(u16 addr){ u16 Va=Read_Dgus(addr); //u16 V1=Read_Dgus(0x0f01); if(Va != 0x0000)

{ if(Va == 0x0001) //復位

{ Write_Dgus(0x1100 + 0, 0); Write_Dgus(0x1100 + 2, 0); Write_Dgus(0x1100 + 4, 0); Write_Dgus(0x1100 + 6, 90); }

else if(Va == 0x0002) //添加

{ if(arm_auto_cnt < 5) ???????????{ ???????????????u8 i = 0; ???????????????u8 send_str[30] = {0}; ???????????????for(i = 0;i < 4;i++) ???????????????{ ???????????????????arm_auto_list[arm_auto_cnt][i] = arm_angle_value[i]; ???????????????} ???????????????sprintf(send_str, "I:%d II:%d III:%d IV:%d", arm_auto_list[arm_auto_cnt][0], arm_auto_list[arm_auto_cnt][1], arm_auto_list[arm_auto_cnt][2], arm_auto_list[arm_auto_cnt][3]); ???????????????write_dgus_vp(0x1500 + 0x20 * arm_auto_cnt, send_str, 16); ???????????????arm_auto_cnt++; ???????????} ???????}?

else if(Va == 0x0003) //開始 { auto_start_flag = 1; auto_start_cnt = 0; }

else if(Va == 0x0004) //停止 { auto_start_flag = 0; Write_Dgus(0x1100 + 0, arm_angle_value_last[0]); Write_Dgus(0x1100 + 2, arm_angle_value_last[1]); Write_Dgus(0x1100 + 4, arm_angle_value_last[2]); Write_Dgus(0x1100 + 6, arm_angle_value_last[3]); }

else if(Va >= 0x0010 && Va <= 0x0014) //刪除內容 ???

{ u16 delete_select = Va - 0x0010; if(arm_auto_cnt > delete_select) //有刪除的內容

{ u8 i = 0; u8 send_str[30] = {0}; for(i = delete_select;i < arm_auto_cnt - 1;i++) ???????????????{ ???????????????????memcpy(arm_auto_list[i], arm_auto_list[i + 1], 8); ???????????????} ???????

memset(arm_auto_list[i], 0, 8); for(i = delete_select;i < arm_auto_cnt - 1;i++) ?????????????

{ sprintf(send_str, "I:%d II:%d III:%d IV:%d\0", arm_auto_list[i][0], arm_auto_list[i][1], arm_auto_list[i][2], arm_auto_list[i][3]); write_dgus_vp(0x1500 + 0x20 * i, send_str, 16); } memset(send_str, 0, 30); write_dgus_vp(0x1500 + 0x20 * i, send_str, 16); arm_auto_cnt--; } } Write_Dgus(addr, 0); }}（4）機械臂自動運動功能//自動運動
void auto_run(){ if(auto_start_flag == 1 && arm_auto_cnt >= 2) //動作必須兩個及以上 { if(memcmp(arm_angle_value, arm_auto_list[auto_start_cnt], 8) != 0) //不等于

{ Write_Dgus(0x1100 + 0, arm_auto_list[auto_start_cnt][0]); Write_Dgus(0x1100 + 2, arm_auto_list[auto_start_cnt][1]); Write_Dgus(0x1100 + 4, arm_auto_list[auto_start_cnt][2]); Write_Dgus(0x1100 + 6, arm_auto_list[auto_start_cnt][3]); }

else if(memcmp(arm_angle_value_last, arm_auto_list[auto_start_cnt], 8) == 0) //當前運動等于目標

{ auto_start_cnt++; if(auto_start_cnt >= arm_auto_cnt)

{ auto_start_cnt = 0; } } }}