一款基于A(yíng)RM的多自由度人形教育機器人控制系統的設計

　　3.3 機器人舵機整體驅動(dòng)函數 ManMoveFrame()設計

　　函數功能：將輸入的各伺服器角度轉換為各伺服器的高電平時(shí)間，按照17個(gè)伺服器高電平時(shí)間從短到長(cháng)依次調用對應伺服器驅動(dòng)。共18個(gè)入口參數，17個(gè)是伺服器角度，1個(gè)是動(dòng)畫(huà)停留時(shí)間，轉換關(guān)系：1度約為0.00814ms。

　　函數定義：void ManMoveFrame(int16 Head_Angle,int16 Larm1_Angle,… ,,int16 Rleg5_Angle,uint8 t)

　　{uint8 i,t1; int HeadTime,Larm1Time,Larm2Time, … ,Rleg4Time,Rleg5Time;/*與角度對應的時(shí)間變量*/

　　int MotorTime[17]; /*各舵機驅動(dòng)脈沖時(shí)間數組*/

　　MotorTime[0]=HeadTime=Head_0+Head_Angle*Angle1;/*頭部驅動(dòng)脈沖時(shí)間*/

　　/*MotorTime[1]至MotorTime[16]略;*/

　　qsort(MotorTime,17,sizeof(int),Compare); /*對MotorTime數組快排序，使其值從小到大重排*/ …

　　for(i=0;i<17;i++)/*循環(huán)17次依次比較并輸出驅動(dòng)脈沖低電平*/

　　{if(MotorTime[i]==HeadTime) head(HeadTime);/* 如果頭部轉動(dòng)角度小則頭部轉動(dòng)結束*/

　　else if(MotorTime[i]== Larm1Time) Larm1(Larm1Time); /*如果左肩轉動(dòng)角度小則頭部轉動(dòng)結束*/

　　… else Rleg5(Rleg5Time);} while(T0TC

　　3.4 機器人姿態(tài)數據輸入驅動(dòng)函數ManMoveKeyframeData()設計

　　機器人動(dòng)作的關(guān)鍵姿態(tài)是由各舵機轉動(dòng)位置決定的，需要參照機器人的體態(tài)對17個(gè)舵機的自由度關(guān)系數據進(jìn)行設置，驅動(dòng)函數將如表2所示設置的參數轉換為驅動(dòng)信號，控制17個(gè)舵機的轉動(dòng)，從而完成機器人行走的動(dòng)作。表格空白的地方默認數據為0。

　　函數功能：初始化各伺服器角度/時(shí)間數組，將機器人每姿態(tài)動(dòng)作的各舵機角度數據輸入給各伺服器數組。共19個(gè)入口參數：1個(gè)關(guān)鍵幀號、17個(gè)伺服器角度、1個(gè)動(dòng)畫(huà)停留時(shí)間。

　　函數定義：void ManMoveKeyframeData(uint16 ID,int16 H_Angle, …,int16 RL5_Angle,uint8 t){

　　ID_Max++;/*動(dòng)作幀數，全局變量，記錄動(dòng)作幀數，每加入1幀，ID_Max的值就增1*/

　　H[ID]=H_Angle;/*頭的角度*/ … RL5[ID]=RL5_Angle; /*左肩的角度*/

　　T_Key[ID]=t;/*幀動(dòng)作完成的時(shí)間長(cháng)度*/ }

　　3.5 機器人動(dòng)作控制驅動(dòng)函數ManMoveKeyframeToFrame()設計

　　主要功能：把動(dòng)作幅度較大的姿態(tài)關(guān)鍵幀數據結合其下面緊挨的姿態(tài)關(guān)鍵幀數據轉換為時(shí)間均勻的普通幀驅動(dòng)參數，供整體驅動(dòng)使用。入口參數：無(wú)。出口參數：無(wú)。