低端微控制器(8bit)的16路舵機調速分析與實現

凝霜發表於2012-04-23

低端微控制器(8bit)的16路舵機調速分析與實現

By 馬冬亮(凝霜  Loki)

一個人的戰爭(http://blog.csdn.net/MDL13412)

        今年的英特爾杯準備做機器人方向,所以在淘寶上買了機器人套件,自己進行組裝和程式設計。機器人裝配完如下圖所示:


        (注:這款機器人由17路舵機組成,其中左右手各2路,左右腿各4路,身體4路,頭部1路。)

        裝配好機器人,首要任務就是讓機器人運動起來。看了一下賣家給的控制程式,寫的很混亂,維護極差,於是準備自己寫一套控制程式,也算是給學弟學妹們留下的一份禮物吧^_^。

        首先簡單介紹一下我們使用的舵機引數:

        *型號.:KCS110M                 
        *速度 :5.0V: 0.14sec/60°
        *扭力 :5.0V: 9.5kg/cm
        *工作電壓:5.0-7.4Volts  
        *轉角範圍  : 0~185度

        由於是控制機器人,所以我對轉角範圍的需求是0~180度,那麼舵機由0度轉到180度需要至少0.6s的時間。

        舵機的調速原理其實很簡單,通過給定連續的PWM訊號,舵機就可以旋轉到相應的角度。我們的這款舵機和競賽的通用舵機基本一致,PWM週期為20ms,復位需要T1=0.5ms的高電平,T2=(20 - 0.5)ms的低電平。每增加1度,需要在T1的基礎上加10us。為了讓舵機完整的轉到對應角度,我們需要至少對舵機連續送0.6s的PWM訊號。

        由於舵機調速對PWM精度要求很高,傳統的12T 8051微控制器每個指令週期佔12個機器週期,速度不能達到要求。我們選擇了STC12C5A60S2這款微控制器,這款微控制器較傳統的12T 8051快至少6倍以上,並配合24MHZ的晶振來控制機器人。

        舵機調速一般有兩種方法,一種是使用定時器中斷進行調速,另外一種是使用軟體延時進行調速。

        首先分析定時器中斷調速的方法:

        我的調速精度要求是1度,所以定時器需要定時10us,那麼,每200次定時器中斷完成一個PWM調速週期。這種程式設計方式的優點是實現簡單,程式碼容易讀懂。其缺點是隻能處理少數舵機,不能對大量舵機進行調速。因為每10us發生一次定時中斷,而每個機器週期為(1 * 1000 * 1000) / (24 * 1000 * 1000)=0.0417us,則在每個定時器中斷中有10 / 0.0417=240個指令週期。我們看下面的程式:

#include <STC_NEW_8051.H>

void main()
{
    volatile int cnt = 0;
    volatile int pwm = 0;
    volatile int pwmCycle = 10;
    
    cnt = (cnt + 1) % 40;
    
    if (++cnt < pwmCycle)
        pwm = 1;
    else
        pwm = 0;
}
        這段程式是模擬定時器中斷中對一路舵機進行調速的情況,注意,這裡一定要使用volatile關鍵字,否則程式會由於編譯器的優化而錯誤。對於所有中斷中用到的全域性變數,我們都需要加上volatile關鍵字防止編譯器優化。下面,我們對這段程式碼進行反彙編,並計算指令週期數。反彙編程式碼如下:

C:0x0097    F50C     MOV      0x0C,A                        3
C:0x0099    750D0A   MOV      0x0D,#0x0A               3

C:0x009C    E509     MOV      A,0x09                        2
C:0x009E    2401     ADD      A,#0x01                       2
C:0x00A0    FF       MOV      R7,A                             2
C:0x00A1    E4       CLR      A                                  1
C:0x00A2    3508     ADDC     A,0x08                        3
C:0x00A4    FE       MOV      R6,A                             2
C:0x00A5    7C00     MOV      R4,#0x00                     2
C:0x00A7    7D28     MOV      R5,#0x28                     2
C:0x00A9    120003   LCALL    C?SIDIV(C:0003)        
C:0x00AC    8C08     MOV      0x08,R4                       3
C:0x00AE    8D09     MOV      0x09,R5                       3

C:0x00B0    0509     INC      0x09                              4
C:0x00B2    E509     MOV      A,0x09                         2
C:0x00B4    7002     JNZ      C:00B8                          3
C:0x00B6    0508     INC      0x08                              4
C:0x00B8    AE08     MOV      R6,0x08                        4
C:0x00BA    C3       CLR      C                                    1
C:0x00BB    950D     SUBB     A,0x0D                         3
C:0x00BD    E50C     MOV      A,0x0C                         2
C:0x00BF    6480     XRL      A,#P0(0x80)                    2
C:0x00C1    F8       MOV      R0,A                               2
C:0x00C2    EE       MOV      A,R6                               1
C:0x00C3    6480     XRL      A,#P0(0x80)                     2
C:0x00C5    98       SUBB     A,R0                                2
C:0x00C6    5007     JNC      C:00CF                            3

C:0x00C8    750A00   MOV      0x0A,#0x00                   3
C:0x00CB    750B01   MOV      0x0B,#0x01                   3
//C:0x00CE    22       RET      
多個if情況時這裡應該是一個跳轉                                  3

C:0x00CF    E4       CLR      A                                      1
C:0x00D0    F50A     MOV      0x0A,A                            3
C:0x00D2    F50B     MOV      0x0B,A                            3
        由以上程式碼可知,一個舵機的調速週期為79+[LCALL C?SIDIV(C:0003) ]條指令週期,所以,對於10us的定時器中斷,最多可以調速2路舵機。

       接下來我們來分析軟體延時的方法進行調速,首先我們需要一個延時函式,那麼我們定義一個延時10us的函式如下所示:

void Delay10us(UINT16 ntimes)
{
    for (delayVar1 = 0; delayVar1 < ntimes; ++delayVar1)
        for (delayVar2 = 0; delayVar2 < 21; ++delayVar2)   
    	    _nop_();     
}
        如果對每個舵機分別進行調速,那麼我們的時間開銷為0.6 * 16 = 9.6s,這個時間是不可接受的。由於8位微控制器有P0-P3的管腳陣列,那麼我們可以利用其中的某一列,同時調速8路舵機,那麼我們的時間開銷為0.6 * 2 = 1.2s,這個對於我們的機器人來說是完全可以接受的。

        同時對8路舵機進行調速的話,我們首先需要對舵機的調速角度進行排序(升序),然後計算出兩兩舵機舵機的差值,這樣就可以同時對8路舵機進行調速了。對於出現的非法角度,我們給這路舵機送全為高電平的PWM訊號,使此路舵機保持先前狀態。程式碼如下所示:

void InitPwmPollint()
{
    UCHAR8 i;
    UCHAR8 j;
    UCHAR8 k;
    UINT16 temp;
    
    for (i = 0; i < USED_PIN; ++i)
    {
        for (j = 0; j < 7; ++j)
        {
            for (k = j; k < 8; ++k)
            {
                if (g_diffAngle[i][j] > g_diffAngle[i][k])
                {
                    temp = g_diffAngle[i][j];
                    g_diffAngle[i][j] = g_diffAngle[i][k];
                    g_diffAngle[i][k] = temp;
                    
                    temp = g_diffAngleMask[i][j];
                    g_diffAngleMask[i][j] = g_diffAngleMask[i][k];
                    g_diffAngleMask[i][k] = temp;
                }
            }
        }
    }
    
    for (i = 0; i < USED_PIN; ++i)
    {
        for (j = 0; j < 8; ++j)
        {
            if (INVALID_JOINT_VALUE == g_diffAngle[i][j])
            {
                g_diffAngle[i][j] = STEERING_ENGINE_FULL_CYCLE;
            }
        }
    }
    
    for (i = 0; i < USED_PIN; ++i)
    {
        for (j = 7; j >= 1; --j)
        {
            g_diffAngle[i][j] = g_diffAngle[i][j] - g_diffAngle[i][j - 1];
        }
    }    
}
        (注:對於上面用到的一些常量,請大家參考本文最後給出的完整程式。

         下面我們舉例說明上述程式碼,假設 g_diffAngle[0][] = {10, 20, 40, 40, 50, 80, 90, 10},那麼經過排序後,其變為g_diffAngle[0][] = {10, 10, 20, 40, 40, 50, 80, 90},而g_diffAngleMask[0][]中的值對應個角度使用的管腳的掩碼,例如P0.0,則其掩碼為0x01,對P0 & 0x01進行掩碼計算,就可以對指定的管腳進行操作。我們對角度排序的同時,需要更新管腳的掩碼,以達到相互對應的目的。

        接下來對角度進行校驗,對於不合法的角度,我們使用STEERING_ENGINE_FULL_CYCLE來進行填充,使這路舵機在PWM調速週期內全為高電平。
       最後計算差值,計算後g_diffAngle[0][]={10, 0, 10, 20, 0, 10, 30, 10},這樣就求出了後面的舵機相對於其前面一路的差值。我們對g_diffAngle[0][0]對應的舵機延時Delay10us(g_diffAngle[0][0])即Delay10us(10),延時完成後我們將這路舵機給低電平,這用到了前面定義的掩碼操作。延時完成後,開始延時g_diffAngle[0][1]對應的舵機,Delay10us(g_diffAngle[0][1])即Delay10us(0),然後將此路舵機送低電平。再延時Delay10us(g_diffAngle[0][2])即Delay10us(10),然後再移除,依次類推。這樣就能保證在一個PWM週期內對8路舵機同時調速。調速部分程式碼如下:

#define PWM_STEERING_ENGINE(group)                                                          \
{                                                                                           \
    counter = STEERING_ENGINE_INIT_DELAY;                                                   \
        for (i = 0; i < 8; ++i)                                                             \
            counter += g_diffAngle[PIN_GROUP_##group][i];                                   \
                                                                                            \
        for (i = 0; i < 30; ++i)                                                            \
        {                                                                                   \
            GROUP_##group##_CONTROL_PIN = 0xFF;                                             \
            Delay10us(STEERING_ENGINE_INIT_DELAY);                                          \
                                                                                            \
            for (j = 0; j < 8; ++j)                                                         \
            {                                                                               \
                Delay10us(g_diffAngle[PIN_GROUP_##group][j]);                               \
                GROUP_##group##_CONTROL_PIN &= ~(g_diffAngleMask[PIN_GROUP_##group][j]);    \
            }                                                                               \
                                                                                            \
            Delay10us(STEERING_ENGINE_CYCLE - counter);                                     \
        }                                                                                   \
}
while (1)
{
    PWM_STEERING_ENGINE(1);
}
        通過上面的操作,我們便完成了同時對8路舵機的調速操作。

        總結:

        對於只有1、2路舵機的情況,我們可以使用定時器中斷進行調速,其具有實現簡單,通俗易懂的有點。而對於多路舵機的情形,就需要使用軟體延時進行操作,其優點是可以同時對8路舵機進行調速,效率高,但是其程式碼不容易實現與除錯。

        附完整程式:

        (注:由於需求變動很大,包括功能和管腳分配等,所以本程式大量使用巨集及enum)

/**
 * @file    ControlRobot.h
 * @author  馬冬亮
 * @brief   用於對機器人進行控制.
 */

#ifndef CONTROLROBOT_H
#define	CONTROLROBOT_H

/**
 * @brief   關節角度非法值.
 * @ingroup ControlRobot
 * 
 * @details 當關節角度無法確定或捕獲關節失敗時, 設定為此數值, 機器人接收到此數值則不轉動舵機.
 */
#define INVALID_JOINT_VALUE 200

/**
 * @brief   定義關節常量用於存取關節角度.
 * @ingroup ControlRobot
 */
typedef enum RobotJoint
{
    ROBOT_LEFT_SHOULDER_VERTICAL      = 0,  ///< 左肩膀, 前後轉動的舵機
    ROBOT_LEFT_SHOULDER_HORIZEN       = 1,  ///< 左肩膀, 上下轉動的舵機
    ROBOT_LEFT_ELBOW                  = 2,  ///< 左肘, 左臂肘關節的舵機

    ROBOT_RIGHT_SHOULDER_VERTICAL     = 3,  ///< 右肩膀, 前後轉動的舵機
    ROBOT_RIGHT_SHOULDER_HORIZEN      = 4,  ///< 右肩膀, 上下轉動的舵機
    ROBOT_RIGHT_ELBOW                 = 5,  ///< 右肘, 右臂肘關節的舵機

    ROBOT_LEFT_HIP_VERTICAL           = 6,  ///< 左髖, 左右轉動的舵機
    ROBOT_LEFT_HIP_HORIZEN            = 7,  ///< 左髖, 前後轉動的舵機
    ROBOT_LEFT_KNEE                   = 8,  ///< 左膝蓋, 左膝關節的舵機
    ROBOT_LEFT_ANKLE                  = 9,  ///< 左踝, 這個舵機不使用

    ROBOT_RIGHT_HIP_VERTICAL          = 10, ///< 右髖, 左右轉動的舵機
    ROBOT_RIGHT_HIP_HORIZEN           = 11, ///< 右髖, 前後轉動的舵機
    ROBOT_RIGHT_KNEE                  = 12, ///< 右膝蓋, 右膝關節的舵機
    ROBOT_RIGHT_ANKLE                 = 13, ///< 右踝, 這個舵機不使用

    ROBOT_LEFT_FOOT                   = 14, ///< 左腳, 這個舵機不使用
    ROBOT_RIGHT_FOOT                  = 15, ///< 右腳, 這個舵機不使用

    ROBOT_HEAD                        = 16, ///< 頭, 這個舵機不使用
        
    ROBOT_JOINT_AMOUNT                = ROBOT_HEAD + 1
} RobotJoint;

typedef enum RobotSteeringEnginePinIndex
{
    ROBOT_PIN_INDEX_LEFT_SHOULDER_VERTICAL                  = 0,
    ROBOT_PIN_INDEX_LEFT_SHOULDER_HORIZEN                   = 1,
    ROBOT_PIN_INDEX_LEFT_ELBOW                              = 2,
    
    ROBOT_PIN_INDEX_RIGHT_SHOULDER_VERTICAL                 = 3,
    ROBOT_PIN_INDEX_RIGHT_SHOULDER_HORIZEN                  = 4,
    ROBOT_PIN_INDEX_RIGHT_ELBOW                             = 5,
    
    ROBOT_PIN_INDEX_LEFT_HIP_VERTICAL                       = 6,
    ROBOT_PIN_INDEX_RIGHT_HIP_VERTICAL                      = 7,
    
    ROBOT_PIN_INDEX_LEFT_HIP_HORIZEN                        = 0,
    ROBOT_PIN_INDEX_LEFT_KNEE                               = 1,
    ROBOT_PIN_INDEX_LEFT_ANKLE                              = 2,
    ROBOT_PIN_INDEX_LEFT_FOOT                               = 3,
    
    ROBOT_PIN_INDEX_RIGHT_HIP_HORIZEN                       = 4,
    ROBOT_PIN_INDEX_RIGHT_KNEE                              = 5,
    ROBOT_PIN_INDEX_RIGHT_ANKLE                             = 6,
    ROBOT_PIN_INDEX_RIGHT_FOOT                              = 7
} RobotSteeringEnginePinIndex;

typedef enum RobotSteeringEnginePinMask
{
    ROBOT_PIN_MASK_LEFT_SHOULDER_VERTICAL                  = 0x01,
    ROBOT_PIN_MASK_LEFT_SHOULDER_HORIZEN                   = 0x02,
    ROBOT_PIN_MASK_LEFT_ELBOW                              = 0x04,
    
    ROBOT_PIN_MASK_RIGHT_SHOULDER_VERTICAL                 = 0x08,
    ROBOT_PIN_MASK_RIGHT_SHOULDER_HORIZEN                  = 0x10,
    ROBOT_PIN_MASK_RIGHT_ELBOW                             = 0x20,
    
    ROBOT_PIN_MASK_LEFT_HIP_VERTICAL                       = 0x40,
    ROBOT_PIN_MASK_RIGHT_HIP_VERTICAL                      = 0x80,
    
    ROBOT_PIN_MASK_LEFT_HIP_HORIZEN                        = 0x01,
    ROBOT_PIN_MASK_LEFT_KNEE                               = 0x02,
    ROBOT_PIN_MASK_LEFT_ANKLE                              = 0x04,
    ROBOT_PIN_MASK_LEFT_FOOT                               = 0x08,
    
    ROBOT_PIN_MASK_RIGHT_HIP_HORIZEN                       = 0x10,
    ROBOT_PIN_MASK_RIGHT_KNEE                              = 0x20,
    ROBOT_PIN_MASK_RIGHT_ANKLE                             = 0x40,
    ROBOT_PIN_MASK_RIGHT_FOOT                              = 0x80
} RobotSteeringEnginePinMask;


    
#define PROTOCOL_HEADER             "\xC9\xCA"
#define PROTOCOL_END                "\xC9\xCB"
#define PROTOCOL_END_LENGTH         2
#define PROTOCOL_HEADER_LENGTH      2
#define COMMAND_STR_LENGTH          (PROTOCOL_HEADER_LENGTH + ROBOT_JOINT_AMOUNT + PROTOCOL_END_LENGTH)
#define COMMAND_STR_BUFFER_SIZE     ((COMMAND_STR_LENGTH) + 2)

#endif	/* CONTROLROBOT_H */
// main.c
#include <STC_NEW_8051.H>
#include "ControlRobot.h"
#include<intrins.h>

#define DEBUG_PROTOCOL

typedef unsigned char UCHAR8;
typedef unsigned int UINT16;

#undef TRUE
#undef FALSE
#define TRUE 1
#define FALSE 0

#define MEMORY_MODEL 

UINT16 MEMORY_MODEL delayVar1;
UCHAR8 MEMORY_MODEL delayVar2;

#define BAUD_RATE 0xF3

#define USED_PIN 2
#define PIN_GROUP_1 0
#define PIN_GROUP_2 1

#define GROUP_1_CONTROL_PIN P0
#define GROUP_2_CONTROL_PIN P2

#define STEERING_ENGINE_CYCLE 2000
#define STEERING_ENGINE_INIT_DELAY 50
#define STEERING_ENGINE_FULL_CYCLE ((STEERING_ENGINE_CYCLE) - (STEERING_ENGINE_INIT_DELAY))

volatile UCHAR8 MEMORY_MODEL g_angle[2][ROBOT_JOINT_AMOUNT];
volatile bit MEMORY_MODEL g_fillingBufferIndex = 0;
volatile bit MEMORY_MODEL g_readyBufferIndex = 1;
volatile bit MEMORY_MODEL g_swapBuffer = FALSE;

volatile UINT16 MEMORY_MODEL g_diffAngle[USED_PIN][8];
volatile UCHAR8 MEMORY_MODEL g_diffAngleMask[USED_PIN][8] = 
{
    {
        ROBOT_PIN_MASK_LEFT_SHOULDER_VERTICAL,  
        ROBOT_PIN_MASK_LEFT_SHOULDER_HORIZEN,  
        ROBOT_PIN_MASK_LEFT_ELBOW,  
        ROBOT_PIN_MASK_RIGHT_SHOULDER_VERTICAL,
        ROBOT_PIN_MASK_RIGHT_SHOULDER_HORIZEN,  
        ROBOT_PIN_MASK_RIGHT_ELBOW,  
        ROBOT_PIN_MASK_LEFT_HIP_VERTICAL,  
        ROBOT_PIN_MASK_RIGHT_HIP_VERTICAL 
    },
    {
        ROBOT_PIN_MASK_LEFT_HIP_HORIZEN,  
        ROBOT_PIN_MASK_LEFT_KNEE,  
        ROBOT_PIN_MASK_LEFT_ANKLE,  
        ROBOT_PIN_MASK_LEFT_FOOT,  
        ROBOT_PIN_MASK_RIGHT_HIP_HORIZEN,  
        ROBOT_PIN_MASK_RIGHT_KNEE,  
        ROBOT_PIN_MASK_RIGHT_ANKLE,  
        ROBOT_PIN_MASK_RIGHT_FOOT
    }
};

#ifdef DEBUG_PROTOCOL
sbit P10 = P1 ^ 0;  // 正在填充交換區1
sbit P11 = P1 ^ 1;  // 正在填充交換區2
sbit P12 = P1 ^ 2;  // 交換區變換
sbit P13 = P1 ^ 3;  // 協議是否正確
#endif 

void Delay10us(UINT16 ntimes)
{
    for (delayVar1 = 0; delayVar1 < ntimes; ++delayVar1)
        for (delayVar2 = 0; delayVar2 < 21; ++delayVar2)   
    	    _nop_();     
}

void InitPwmPollint()
{
    UCHAR8 i;
    UCHAR8 j;
    UCHAR8 k;
    UINT16 temp;
    
    for (i = 0; i < USED_PIN; ++i)
    {
        for (j = 0; j < 7; ++j)
        {
            for (k = j; k < 8; ++k)
            {
                if (g_diffAngle[i][j] > g_diffAngle[i][k])
                {
                    temp = g_diffAngle[i][j];
                    g_diffAngle[i][j] = g_diffAngle[i][k];
                    g_diffAngle[i][k] = temp;
                    
                    temp = g_diffAngleMask[i][j];
                    g_diffAngleMask[i][j] = g_diffAngleMask[i][k];
                    g_diffAngleMask[i][k] = temp;
                }
            }
        }
    }
    
    for (i = 0; i < USED_PIN; ++i)
    {
        for (j = 0; j < 8; ++j)
        {
            if (INVALID_JOINT_VALUE == g_diffAngle[i][j])
            {
                g_diffAngle[i][j] = STEERING_ENGINE_FULL_CYCLE;
            }
        }
    }
    
    for (i = 0; i < USED_PIN; ++i)
    {
        for (j = 7; j >= 1; --j)
        {
            g_diffAngle[i][j] = g_diffAngle[i][j] - g_diffAngle[i][j - 1];
        }
    }    
}


void InitSerialPort()
{
    AUXR    = 0x00;
    ES      = 0;
    TMOD    = 0x20;
    SCON    = 0x50;
    TH1     = BAUD_RATE;
    TL1     = TH1;
    PCON    = 0x80;
    EA      = 1;
    ES      = 1;
    TR1     = 1;
}

void OnSerialPort() interrupt 4
{
    static UCHAR8 previousChar = 0;
    static UCHAR8 currentChar = 0;
    static UCHAR8 counter = 0;

    if (RI)
    {
        RI = 0;
        currentChar = SBUF;

        if (PROTOCOL_HEADER[0] == currentChar)    	 // 協議標誌
        {
            previousChar =     currentChar;
        }
        else
        {
            if (PROTOCOL_HEADER[0] == previousChar && PROTOCOL_HEADER[1] == currentChar)       // 協議開始
            {
                counter = 0;
                previousChar = currentChar;
                g_swapBuffer = FALSE;
            }
            else if (PROTOCOL_END[0] == previousChar && PROTOCOL_END[1] == currentChar)   // 協議結束
            {
                previousChar = currentChar;

                if (ROBOT_JOINT_AMOUNT == counter)    	// 協議接受正確
                {
                    if (0 == g_fillingBufferIndex)
                    {
                        g_readyBufferIndex = 0;
                        g_fillingBufferIndex = 1;
                    }
                    else
                    {
                        g_readyBufferIndex = 1;
                        g_fillingBufferIndex = 0;
                    }

                    g_swapBuffer = TRUE;
    
#ifdef DEBUG_PROTOCOL
                    P13 = 0;
#endif
                }
                else
                {
                    g_swapBuffer = FALSE;
                    
#ifdef DEBUG_PROTOCOL
                    P13 = 1;
#endif
                }

                counter = 0;
            }
            else   // 接受協議正文
            {
                g_swapBuffer = FALSE;
                previousChar = currentChar;

                if (counter < ROBOT_JOINT_AMOUNT)
                     g_angle[g_fillingBufferIndex][counter] = currentChar;

                ++counter;
            }
        }    // if (PROTOCOL_HEADER[0] == currentChar)

        SBUF = currentChar;
        while (!TI)
            ;
        TI = 0;
    }    // (RI)
}

void FillDiffAngle()
{
    // 設定舵機要調整的角度
    g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_VERTICAL] = g_angle[g_readyBufferIndex][ROBOT_LEFT_SHOULDER_VERTICAL];
    g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_HORIZEN] = g_angle[g_readyBufferIndex][ROBOT_LEFT_SHOULDER_HORIZEN];
    g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_ELBOW] = g_angle[g_readyBufferIndex][ROBOT_LEFT_ELBOW];

    g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_VERTICAL] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_SHOULDER_VERTICAL];
    g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_HORIZEN] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_SHOULDER_HORIZEN];
    g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_ELBOW] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_ELBOW];

    g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_HIP_VERTICAL] = g_angle[g_readyBufferIndex][ROBOT_LEFT_HIP_VERTICAL];
    g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_HIP_VERTICAL] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_HIP_VERTICAL];
    
    g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_HIP_HORIZEN] = g_angle[g_readyBufferIndex][ROBOT_LEFT_HIP_HORIZEN];
    g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_KNEE] = g_angle[g_readyBufferIndex][ROBOT_LEFT_KNEE];
    g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_ANKLE] = g_angle[g_readyBufferIndex][ROBOT_LEFT_ANKLE];
    g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_FOOT] = g_angle[g_readyBufferIndex][ROBOT_LEFT_FOOT];

    g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_HIP_HORIZEN] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_HIP_HORIZEN];
    g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_KNEE] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_KNEE];
    g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_ANKLE] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_ANKLE];
    g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_FOOT] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_FOOT];

    // 復位舵機管腳索引
    g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_VERTICAL] = ROBOT_PIN_MASK_LEFT_SHOULDER_VERTICAL;
    g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_HORIZEN] = ROBOT_PIN_MASK_LEFT_SHOULDER_HORIZEN;
    g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_ELBOW] = ROBOT_PIN_MASK_LEFT_ELBOW;

    g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_VERTICAL] = ROBOT_PIN_MASK_RIGHT_SHOULDER_VERTICAL;
    g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_HORIZEN] = ROBOT_PIN_MASK_RIGHT_SHOULDER_HORIZEN;
    g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_ELBOW] = ROBOT_PIN_MASK_RIGHT_ELBOW;

    g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_HIP_VERTICAL] = ROBOT_PIN_MASK_LEFT_HIP_VERTICAL;
    g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_HIP_VERTICAL] = ROBOT_PIN_MASK_RIGHT_HIP_VERTICAL;
    
    g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_HIP_HORIZEN] = ROBOT_PIN_MASK_LEFT_HIP_HORIZEN;
    g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_KNEE] = ROBOT_PIN_MASK_LEFT_KNEE;
    g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_ANKLE] = ROBOT_PIN_MASK_LEFT_ANKLE;
    g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_FOOT] = ROBOT_PIN_MASK_LEFT_FOOT;

    g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_HIP_HORIZEN] = ROBOT_PIN_MASK_RIGHT_HIP_HORIZEN;
    g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_KNEE] = ROBOT_PIN_MASK_RIGHT_KNEE;
    g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_ANKLE] = ROBOT_PIN_MASK_RIGHT_ANKLE;
    g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_FOOT] = ROBOT_PIN_MASK_RIGHT_FOOT;
}

#define PWM_STEERING_ENGINE(group)                                                          \
{                                                                                           \
    counter = STEERING_ENGINE_INIT_DELAY;                                                   \
        for (i = 0; i < 8; ++i)                                                             \
            counter += g_diffAngle[PIN_GROUP_##group][i];                                   \
                                                                                            \
        for (i = 0; i < 30; ++i)                                                            \
        {                                                                                   \
            GROUP_##group##_CONTROL_PIN = 0xFF;                                             \
            Delay10us(STEERING_ENGINE_INIT_DELAY);                                          \
                                                                                            \
            for (j = 0; j < 8; ++j)                                                         \
            {                                                                               \
                Delay10us(g_diffAngle[PIN_GROUP_##group][j]);                               \
                GROUP_##group##_CONTROL_PIN &= ~(g_diffAngleMask[PIN_GROUP_##group][j]);    \
            }                                                                               \
                                                                                            \
            Delay10us(STEERING_ENGINE_CYCLE - counter);                                     \
        }                                                                                   \
}

void main()
{
    UCHAR8 i;
    UCHAR8 j;
    UINT16 counter;

    InitSerialPort();
    
    P1 = 0xFF;

    // 初始化舵機角度
    for (i = 0; i < ROBOT_JOINT_AMOUNT; ++i)
    {
        g_angle[0][i] = 45;
        g_angle[1][i] = 45;
    }
    
    for (i = 0; i < USED_PIN; ++i)
        for (j = 0; j < 8; ++j)
            g_diffAngle[i][j] = 0;
            
    FillDiffAngle();
    InitPwmPollint();

    while (1)
    {
#ifdef DEBUG_PROTOCOL
        if (g_fillingBufferIndex)
        {
            P11 = 0;
            P10 = 1;
        }
        else
        {
            P11 = 1;
            P10 = 0;
        }

        if (g_swapBuffer)
            P12 = 0;
        else
            P12 = 1;
#endif

        if (g_swapBuffer)
        {
            FillDiffAngle();
            
            g_swapBuffer = FALSE;
            
            InitPwmPollint();
        }
                
        PWM_STEERING_ENGINE(1)
        PWM_STEERING_ENGINE(2)
    }

}

相關文章