六足机器人的程序

1. 六足机器人主程序main.c

六足机器人主程序main.c__________点击查看代码

`/* Includes ------------------------------------------------------------------*/
#include "stm32f10x_lib.h"
#include "stm32f10x_init.h"
#include "spider.h"
#include "spidertrack.h"
#include "camera.h"
#include "gps.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
#ifndef RxBuffersize
#define RxBufferSize  5000
#endif

/* Private variables ---------------------------------------------------------*/
SPIDER MySpider;
SPIDER TmpSpider;
TRACKBUFFER MyTrackBuf;
CAMERA MyCamera;
GPS MyGPS;

extern vu8  RxBuffer[];
extern vu16 RxCounter ;
extern vu8  USART3_RXNE_DIS_FLAG ;

extern vu8 WiFiBuffer[];
extern vu8 wificount;
vu8 wifircv = 0;
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
void CntIncwithCheck(vu8* cnt)
{
    *cnt++;
    if(*cnt >= 100)
    {
        *cnt = 0;
    }
}
/*******************************************************************************
* Function Name  : main
* Description    : Main program.
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
int main(void)
{
    u16 i = 0;
    double height = 30.0;
    u16 counter = 0;
    u16 tmp = 0;
    u16 angle = 0;

    SPDRStructInit(&MySpider);
    CameInit(&MyCamera);
    GPSInit(&MyGPS);
    Stm32f10x_Configuration(&MySpider);
    TIM8_Configuration(&MyCamera);
    Stm32f10x_Initialization();

    SPDRChangeHeight(&MySpider, height);
    TRACK_CreateAngle(&MySpider, &(MyTrackBuf.Trck));
    TRACK_TransAngleToPulse(&MyTrackBuf,0,1);
    SPDRRenovateAllData_S(&MySpider, MyTrackBuf.JointPulse[0]);
    SPDRRenovateAllData_H(&MySpider);
    Delay(1000);
	for(i=0; i<20; i++)
    {
        height += 2.0;
        SPDRChangeHeight(&MySpider, height);
        TRACK_CreateAngle(&MySpider, &(MyTrackBuf.Trck));
        TRACK_TransAngleToPulse(&MyTrackBuf,0,1);
        SPDRRenovateAllData_S(&MySpider, MyTrackBuf.JointPulse[0]);
        SPDRRenovateAllData_H(&MySpider);
        Delay(100);
    }
    height = 0.0;
    TmpSpider = MySpider;
    //Delay(1000);
    TRACK_TurnRightStep(&MySpider, &TmpSpider, &MyTrackBuf);
    MySpider = TmpSpider;
    TRACK_TurnLeftStep(&MySpider, &TmpSpider, &MyTrackBuf);
    MySpider = TmpSpider;
    TRACK_StraightStep(&MySpider, &TmpSpider, &MyTrackBuf);
    Delay(1000);

    TRACK_TurnRight(&MySpider, &MyTrackBuf);
    Delay(100);
    TRACK_TurnLeft(&MySpider, &MyTrackBuf);
    Delay(100);
    TRACK_Straight(&MySpider, &MyTrackBuf);
	Delay(100);
	TRACK_Straight(&MySpider, &MyTrackBuf);
	Delay(100);
    TRACK_TurnRight(&MySpider, &MyTrackBuf);
    Delay(100);
    TRACK_TurnLeft(&MySpider, &MyTrackBuf);
    SFTMRStart(10000);
   	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
    USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
    while(1)
    {
        if(1 == SFTMRCheck())
        {
        if(USART3_RXNE_DIS_FLAG == 1)
        {
            for(counter=0; counter<RxBufferSize; counter++)
            {
                GPSJudge(&MyGPS,RxBuffer[counter]);
            }
            /** send start flag # */
            USART_SendData(USART1, '#');
            while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET)
            {}
            for(counter=0; counter<11; counter++)
            {
                if(counter == 1)
                {
                    USART_SendData(USART1, 0x30);
                    while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET)
                    {}
                }
                USART_SendData(USART1, MyGPS.GPSLatitude[counter]);
                while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET)
                {}
            }
            /** send over flag $*/
            USART_SendData(USART1, '$');
            while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET)
            {}
           

            /** send start flag # */
            USART_SendData(USART1, '#');
            while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET)
            {}
            for(counter=0; counter<12; counter++)
            {

                USART_SendData(USART1, MyGPS.GPSLongitude[counter]);
                while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET)
                {}
            }
            /** send over flag $*/
            USART_SendData(USART1, '$');
            while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET)
            {}
            /**/

            USART3_RXNE_DIS_FLAG = 0;
            USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
        }
        SFTMRStart(10000);
        }

        if(wifircv != wificount)
        {
        /* Read one byte from the receive data register */
            if(WiFiBuffer[wifircv] == 'H')
            {
                CntIncwithCheck(&wifircv);
                angle = 0;
                for(tmp=1; tmp<4; tmp++)
                {
                    angle = angle * 10 + ( WiFiBuffer[wifircv] - 0x30 );
                    CntIncwithCheck(&wifircv);
                }
                CameMoveHorizontal(&MyCamera,angle);
                angle = 0;
            }
            else if(WiFiBuffer[tmp] == 'V')
            {
                CntIncwithCheck(&wifircv);
                angle = 0;
                for(tmp=1; tmp<4; tmp++)
                {
                    angle = angle * 10 + ( WiFiBuffer[wifircv] - 0x30 );
                    CntIncwithCheck(&wifircv);
                }
                CameMoveVertical(&MyCamera,angle);
                angle = 0;
            }
            else if(WiFiBuffer[tmp] == 'S')
            {
                CntIncwithCheck(&wifircv);
                angle = 0;
                for(tmp=1; tmp<4; tmp++)
                {
                    angle = angle * 10 + ( WiFiBuffer[wifircv] - 0x30 );
                    CntIncwithCheck(&wifircv);
                }
                switch(angle)
                {
                case 0x01://move
                    TRACK_Straight(&MySpider, &MyTrackBuf);
                    break;
                case 0x02://turn left
                    TRACK_TurnLeft(&MySpider, &MyTrackBuf);
                    break;
                case 0x03://turn right
                    TRACK_TurnRight(&MySpider, &MyTrackBuf);
                    break;
                case 0x04://stop
                    break;
                default:
                    break;
                }
                angle = 0;
            }
            else
            {
                CntIncwithCheck(&wifircv);
            }
        }
    }

}


#ifdef  DEBUG

void assert_failed(u8* file, u32 line)
{
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

  /* Infinite loop */
  while (1)
  {
  }
}
#endif

/******************* (C) COPYRIGHT 2008 STMicroelectronics *****END OF FILE****/`

2. 六足机器人的十八个电机（自由度）的定义 spider.c和spider.h

六足机器人的基础数据自定义spider.c__________点击查看代码

#include "spider.h"
#include "stm32f10x_init.h"

void SPDRStructInit(SPIDER* spdr)
{
    spdr->Spdr_State = SPDR_STAT_NONE;
    spdr->Spdr_Height = 30.0;

    spdr->Right_Front_Leg.Height = spdr->Spdr_Height;
    spdr->Right_Front_Leg.Length = 140.0;
    spdr->Right_Front_Leg.anglenear = 90.0;
    spdr->Right_Front_Leg.Joint_Far  = 540;
    spdr->Right_Front_Leg.Joint_Mid  = 540;
    spdr->Right_Front_Leg.Joint_Near = 540;

    spdr->Right_Middle_Leg.Height = spdr->Spdr_Height;
    spdr->Right_Middle_Leg.Length = 140.0;
    spdr->Right_Middle_Leg.anglenear = 90.0;
    spdr->Right_Middle_Leg.Joint_Far  = 540;
    spdr->Right_Middle_Leg.Joint_Mid  = 540;
    spdr->Right_Middle_Leg.Joint_Near = 540;

    spdr->Right_Back_Leg.Height = spdr->Spdr_Height;
    spdr->Right_Back_Leg.Length = 120.0;
    spdr->Right_Back_Leg.anglenear = 90.0;
    spdr->Right_Back_Leg.Joint_Far  = 540;
    spdr->Right_Back_Leg.Joint_Mid  = 540;
    spdr->Right_Back_Leg.Joint_Near = 540;

    spdr->Left_Front_Leg.Height = spdr->Spdr_Height;
    spdr->Left_Front_Leg.Length = 140.0;
    spdr->Left_Front_Leg.anglenear = 90.0;
    spdr->Left_Front_Leg.Joint_Far  = 540;
    spdr->Left_Front_Leg.Joint_Mid  = 540;
    spdr->Left_Front_Leg.Joint_Near = 540;

    spdr->Left_Middle_Leg.Height = spdr->Spdr_Height;
    spdr->Left_Middle_Leg.Length = 140.0;
    spdr->Left_Middle_Leg.anglenear = 90.0;
    spdr->Left_Middle_Leg.Joint_Far  = 540;
    spdr->Left_Middle_Leg.Joint_Mid  = 540;
    spdr->Left_Middle_Leg.Joint_Near = 540;

    spdr->Left_Back_Leg.Height = spdr->Spdr_Height;
    spdr->Left_Back_Leg.Length = 120.0;
    spdr->Left_Back_Leg.anglenear = 90.0;
    spdr->Left_Back_Leg.Joint_Far  = 540;
    spdr->Left_Back_Leg.Joint_Mid  = 540;
    spdr->Left_Back_Leg.Joint_Near = 540;
}


void SPDRChangeHeight(SPIDER *spdr,double newheight)
{
    spdr->Spdr_Height = newheight;
    spdr->Right_Front_Leg.Height = newheight;
    spdr->Right_Middle_Leg.Height = newheight;
    spdr->Right_Back_Leg.Height = newheight;
    spdr->Left_Front_Leg.Height = newheight;
    spdr->Left_Middle_Leg.Height = newheight;
    spdr->Left_Back_Leg.Height = newheight;
}

void SPDRRenovateAllData_H(SPIDER* spdr)
{
    TIM_RFLJfarRenovate(spdr->Right_Front_Leg.Joint_Far);
    TIM_RFLJmidRenovate(spdr->Right_Front_Leg.Joint_Mid);
    TIM_RFLJnearRenovate(spdr->Right_Front_Leg.Joint_Near);

    TIM_RMLJfarRenovate(spdr->Right_Middle_Leg.Joint_Far);
    TIM_RMLJmidRenovate(spdr->Right_Middle_Leg.Joint_Mid);
    TIM_RMLJnearRenovate(spdr->Right_Middle_Leg.Joint_Near);

    TIM_RBLJfarRenovate(spdr->Right_Back_Leg.Joint_Far);
    TIM_RBLJmidRenovate(spdr->Right_Back_Leg.Joint_Mid);
    TIM_RBLJnearRenovate(spdr->Right_Back_Leg.Joint_Near);

    TIM_LFLJfarRenovate(spdr->Left_Front_Leg.Joint_Far);
    TIM_LFLJmidRenovate(spdr->Left_Front_Leg.Joint_Mid);
    TIM_LFLJnearRenovate(spdr->Left_Front_Leg.Joint_Near);

    TIM_LMLJfarRenovate(spdr->Left_Middle_Leg.Joint_Far);
    TIM_LMLJmidRenovate(spdr->Left_Middle_Leg.Joint_Mid);
    TIM_LMLJnearRenovate(spdr->Left_Middle_Leg.Joint_Near);

    TIM_LBLJfarRenovate(spdr->Left_Back_Leg.Joint_Far);
    TIM_LBLJmidRenovate(spdr->Left_Back_Leg.Joint_Mid);
    TIM_LBLJnearRenovate(spdr->Left_Back_Leg.Joint_Near);
}

void SPDRRenovateAllData_S(SPIDER* spdr,u16* array)
{
    spdr->Right_Front_Leg.Joint_Far  = array[0];
    spdr->Right_Front_Leg.Joint_Mid  = array[1];
    spdr->Right_Front_Leg.Joint_Near = array[2];
    spdr->Right_Middle_Leg.Joint_Far = array[3];
    spdr->Right_Middle_Leg.Joint_Mid = array[4];
    spdr->Right_Middle_Leg.Joint_Near= array[5];
    spdr->Right_Back_Leg.Joint_Far   = array[6];
    spdr->Right_Back_Leg.Joint_Mid   = array[7];
    spdr->Right_Back_Leg.Joint_Near  = array[8];
    spdr->Left_Front_Leg.Joint_Far   = array[9];
    spdr->Left_Front_Leg.Joint_Mid   = array[10];
    spdr->Left_Front_Leg.Joint_Near  = array[11];
    spdr->Left_Middle_Leg.Joint_Far  = array[12];
    spdr->Left_Middle_Leg.Joint_Mid  = array[13];
    spdr->Left_Middle_Leg.Joint_Near = array[14];
    spdr->Left_Back_Leg.Joint_Far    = array[15];
    spdr->Left_Back_Leg.Joint_Mid    = array[16];
    spdr->Left_Back_Leg.Joint_Near   = array[17];
}

六足机器人的基础数据自定义spider.h__________点击查看代码

#ifndef __SPIDER_H
#define __SPIDER_H

#include "stm32f10x_lib.h"


typedef enum
{
    SPDR_STAT_NONE,
    SPDR_STAT_STANDUP,
    SPDR_STAT_WALKSTRAIGHT,
    SPDR_STAT_TURNLEFT,
    SPDR_STAT_TURNRIGHT,
    SPDR_STAT_DRAW
}SPIDER_State;


typedef struct
{
    u16 Joint_Far;
    u16 Joint_Mid;
    u16 Joint_Near;
    double Length;
    double Height; /** measure by milimeter */
    double anglenear;
}SPIDER_Leg;

typedef struct
{
    SPIDER_Leg   Right_Front_Leg;
    SPIDER_Leg   Right_Middle_Leg;
    SPIDER_Leg   Right_Back_Leg;
    SPIDER_Leg   Left_Front_Leg;
    SPIDER_Leg   Left_Middle_Leg;
    SPIDER_Leg   Left_Back_Leg;
    SPIDER_State Spdr_State;
    double       Spdr_Height;
    void         (*Spdr_Action)();
}SPIDER;


void SPDRStructInit(SPIDER* spdr);
void SPDRChangeHeight(SPIDER *spdr,double newheight);
void SPDRIdle(void);
void SPDRWalkStraight(void);
void SPDRTurnLeft(void);
void SPDRTurnRight(void);
void SPDRDrawLine(u16 length);
/** _S indicate for software */
void SPDRRenovateAllData_S(SPIDER* spdr,u16* array);
/** _H indicate for hardware */
void SPDRRenovateAllData_H(SPIDER* spdr);
void SPDRPrepareTurnLeft(SPIDER* spdr);


#endif // __SPIDER_H

3. 六足步态程序spidertrack.c和spidertrack.h

六足步态程序spidertrack.c__________点击查看代码

#include "spidertrack.h"
#include "stm32f10x_lib.h"
#include <math.h>
#include "stm32f10x_init.h"

#define COMPUTEEDGE(A,b,c)  (sqrt((b) * (b) + (c) * (c) - 2 * (b) * (c) * cos(A)))
#define COMPUTEANGLE(a,b,c) (acos(((b) * (b) + (c) * (c) - (a) * (a)) / (2 * (b) * (c))))
#define RFLZEROOFFSET       (PI / 4 + (atan(1/3))/2)
#define RBLZEROOFFSET       (PI / 4 + (atan(1/3))/2)
#define LFLZEROOFFSET       (PI / 4 + (atan(1/3))/2)
#define LBLZEROOFFSET       (PI / 4 + (atan(1/3))/2)

void TRACK_CreateAngle(SPIDER* spdr, TRACK* trck)
{
    /** RFL the orginal angle is suit the far and mid joints */
    TRACK_LengthToAngle(spdr->Right_Front_Leg.Height, spdr->Right_Front_Leg.Length,
                        &(trck->jointangle[0]), &(trck->jointangle[1]));
    //trck->jointangle[0] = 180 - trck->jointangle[0];
    trck->jointangle[2] = spdr->Right_Front_Leg.anglenear;
    /** RML the orginal angle is suit the far and mid joints */
    TRACK_LengthToAngle(spdr->Right_Middle_Leg.Height, spdr->Right_Middle_Leg.Length,
                        &(trck->jointangle[3]), &(trck->jointangle[4]));
    //trck->jointangle[3] = 180 - trck->jointangle[3];
    trck->jointangle[5] = spdr->Right_Middle_Leg.anglenear;
    /** RBL the orginal angle is suit the far and mid joints */
    TRACK_LengthToAngle(spdr->Right_Back_Leg.Height, spdr->Right_Back_Leg.Length,
                        &(trck->jointangle[6]), &(trck->jointangle[7]));
    //trck->jointangle[6] = 180 - trck->jointangle[6];
    trck->jointangle[8] = spdr->Right_Back_Leg.anglenear;
    /** LFL the orginal angle doesn't suit any joint,both should be minused by 180.*/
    TRACK_LengthToAngle(spdr->Left_Front_Leg.Height, spdr->Left_Front_Leg.Length,
                        &(trck->jointangle[9]), &(trck->jointangle[10]));
    trck->jointangle[9] = 180 - trck->jointangle[9];
    trck->jointangle[10] = 180 - trck->jointangle[10];
    trck->jointangle[11] = spdr->Left_Front_Leg.anglenear;
     /** LFL the orginal angle doesn't suit any joint,both should be minused by 180.*/
    TRACK_LengthToAngle(spdr->Left_Middle_Leg.Height, spdr->Left_Middle_Leg.Length,
                        &(trck->jointangle[12]), &(trck->jointangle[13]));
    trck->jointangle[12] = 180 - trck->jointangle[12];
    trck->jointangle[13] = 180 - trck->jointangle[13];
    trck->jointangle[14] = spdr->Left_Middle_Leg.anglenear;
     /** LFL the orginal angle doesn't suit any joint,both should be minused by 180.*/
    TRACK_LengthToAngle(spdr->Left_Back_Leg.Height, spdr->Left_Back_Leg.Length,
                        &(trck->jointangle[15]), &(trck->jointangle[16]));
    trck->jointangle[15] = 180 - trck->jointangle[15];
    trck->jointangle[16] = 180 - trck->jointangle[16];
    trck->jointangle[17] = spdr->Left_Back_Leg.anglenear;
}

void TRACK_LengthToAngle(double height, double length, double* anglefar, double* anglemid)
{
	double th1bar,th2bar;
	double f1,f2;
	double det;
	double dth1,dth2;
	double epsilon=0.1;

	th1bar=-100*PI/180;
	th2bar=135*PI/180;

	f1=120*sin(th1bar)+85*sin(th2bar)+height;
	f2=120*cos(th1bar)+85*cos(th2bar)+length;
	while(f1>epsilon||f2>epsilon)
	{
		det=10200*(sin(th1bar)*cos(th2bar)-cos(th1bar)*sin(th2bar));
		dth1=85/det*(sin(th2bar)*f1+cos(th2bar)*f2);
		dth2=-120/det*(sin(th1bar)*f1+cos(th1bar)*f2);
		th1bar+=dth1;
		th2bar+=dth2;
		f1=120*sin(th1bar)+85*sin(th2bar)+height;
		f2=120*cos(th1bar)+85*cos(th2bar)+length;
	}
	//将弧度值由换算回角度值
	*anglefar = (th2bar-th1bar)*180/PI-180;
	/** thus the far angle is measured between the two bars which forms
        this angle */
	*anglemid = th2bar*180/PI - 45;
	/** thus the mid angle is measured from the vertical
        axis, ie the zero angle is from vertical,for the
        sevo action from the vertical */
}

void TRACK_TransAngleToPulse(TRACKBUFFER* trckbuf,u16 cnt,u8 type)
{
    u8 i = 0;
    switch(type)
    {
    case 1:
        for(i=0; i<18; i++)
        {
            trckbuf->JointPulse[cnt][i] = ceil(trckbuf->Trck.jointangle[i] * 4 + 180);
        }
        break;
    case 2:
        for(i=0; i<18; i++)
        {
            trckbuf->JointPulseTLP[cnt][i] = ceil(trckbuf->Trck.jointangle[i] * 4 + 180);
        }
        break;
    case 3:
        for(i=0; i<18; i++)
        {
            trckbuf->JointPulseTL[cnt][i] = ceil(trckbuf->Trck.jointangle[i] * 4 + 180);
        }
        break;
    case 4:
        for(i=0; i<18; i++)
        {
            trckbuf->JointPulseTR[cnt][i] = ceil(trckbuf->Trck.jointangle[i] * 4 + 180);
        }
    default:
        break;
    }

}

/** @arg legheight positive value */
void TRACK_StraightStepVerticalA(SPIDER* spdr, double legheight)
{
    spdr->Right_Middle_Leg.Height = legheight;
    spdr->Left_Front_Leg.Height = legheight;
    spdr->Left_Back_Leg.Height = legheight;
}


void TRACK_StraightStepHorzontalA(SPIDER* spdr, double steplength)
{
    /** Right middle leg */
    double oldangle  = spdr->Right_Middle_Leg.anglenear;
    double oldlength = spdr->Right_Middle_Leg.Length;
    double newlength = 0.0;
    double newangle  = 0.0;
    oldangle = oldangle * PI / 180;
    newlength = COMPUTEEDGE(oldangle, oldlength, steplength);
    newangle  = COMPUTEANGLE(oldlength, newlength, steplength);
    newangle  = (PI - newangle) * 180 / PI;
    spdr->Right_Middle_Leg.Length = newlength;
    spdr->Right_Middle_Leg.anglenear = newangle;
    /** Left front leg */
    oldangle = spdr->Left_Front_Leg.anglenear;
    oldlength = spdr->Left_Front_Leg.Length;
    oldangle  = oldangle * PI / 180	;
    oldangle  = PI - (oldangle + LFLZEROOFFSET);
    newlength = COMPUTEEDGE(oldangle, oldlength, steplength);
    newangle  = COMPUTEANGLE(oldlength, newlength, steplength);
    newangle  = (newangle - LFLZEROOFFSET) * 180 / PI;
    spdr->Left_Front_Leg.anglenear = newangle;
    spdr->Left_Front_Leg.Length = newlength;
    /** Left back leg */
    oldangle = spdr->Left_Back_Leg.anglenear;
    oldlength = spdr->Left_Back_Leg.Length;
    oldangle  = oldangle * PI / 180;
    oldangle  = PI - oldangle + LBLZEROOFFSET;
    newlength = COMPUTEEDGE(oldangle, oldlength, steplength);
    newangle  = COMPUTEANGLE(oldlength, newlength, steplength);
    newangle  = (newangle + LBLZEROOFFSET) * 180 / PI;
    spdr->Left_Back_Leg.anglenear = newangle;
    spdr->Left_Back_Leg.Length = newlength;
}

void TRACK_StraightStepVerticalB(SPIDER* spdr, double legheight)
{
    spdr->Left_Middle_Leg.Height = legheight;
    spdr->Right_Front_Leg.Height = legheight;
    spdr->Right_Back_Leg.Height = legheight;
}

void TRACK_StraightStepHorzontalB(SPIDER* spdr, double steplength)
{
    /** Right front leg */
    double oldangle  = spdr->Right_Front_Leg.anglenear;
    double oldlength = spdr->Right_Front_Leg.Length;
    double newlength = 0.0;
    double newangle  = 0.0;
    oldangle = oldangle * PI / 180 - RFLZEROOFFSET;
    newlength = COMPUTEEDGE(oldangle, oldlength, steplength);
    newangle  = COMPUTEANGLE(oldlength, newlength, steplength);
    newangle  = PI - newangle;
    newangle  = (newangle + RFLZEROOFFSET) * 180 / PI ;
    spdr->Right_Front_Leg.Length = newlength;
    spdr->Right_Front_Leg.anglenear = newangle;
    /** Right back leg */
    oldangle  = spdr->Right_Back_Leg.anglenear;
    oldlength = spdr->Left_Back_Leg.Length;
    oldangle  = oldangle * PI / 180 + RBLZEROOFFSET;
    newlength = COMPUTEEDGE(oldangle, oldlength, steplength);
    newangle  = COMPUTEANGLE(oldlength, newlength, steplength);
    newangle  = PI - newangle;
    newangle  = (newangle - RBLZEROOFFSET) * 180 / PI;
    spdr->Right_Back_Leg.anglenear = newangle;
    spdr->Left_Back_Leg.Length = newlength;
    /** Left middle leg */
    oldangle = spdr->Left_Middle_Leg.anglenear;
    oldlength = spdr->Left_Middle_Leg.Length;
    oldangle  = PI - oldangle * PI / 180;
    newlength = COMPUTEEDGE(oldangle, oldlength, steplength);
    newangle  = COMPUTEANGLE(oldlength, newlength, steplength);
    newangle  = newangle * 180 / PI;
    spdr->Left_Middle_Leg.anglenear = newangle;
    spdr->Left_Middle_Leg.Length = newlength;
}


void TRACK_StraightStep(SPIDER* spdr, SPIDER* original, TRACKBUFFER* trckbuf)
{
    volatile SPIDER tmpspdr;
    double legheight = spdr->Spdr_Height;
    double steplength = 0.0;
    double tmp[6] = {0};
	u16 i = 0;
	u16 cnt = 0;
    /** A group legs up 5.0 centimeters */
    for(i=0; i<10; i++)
    {
        legheight -= 5.0;
        TRACK_StraightStepVerticalA(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 1);
        cnt++;
    }
    tmpspdr = *spdr;
    /** B group legs drive the body forward */
    for(i=0; i<5; i++)
    {
        steplength += 1.0;
        TRACK_StraightStepHorzontalB(spdr, steplength);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 1);
        cnt++;
    }
    /** A group legs down 5.0 centimeters */
    for(i=0; i<10; i++)
    {
        legheight += 5.0;
        TRACK_StraightStepVerticalA(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 1);
        cnt++;
    }
    /** B group legs up 5.0 centimeters and return to normal state */
    for(i=0; i<10; i++)
    {
        legheight -= 5.0;
        TRACK_StraightStepVerticalB(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 1);
        cnt++;
    }
    tmp[0] = (spdr->Right_Front_Leg.anglenear - tmpspdr.Right_Front_Leg.anglenear) / 10;
    tmp[1] = (spdr->Right_Back_Leg.anglenear - tmpspdr.Right_Back_Leg.anglenear) / 10;
    tmp[2] = (spdr->Left_Middle_Leg.anglenear - tmpspdr.Left_Middle_Leg.anglenear) / 10;
    tmp[3] = (spdr->Right_Front_Leg.Length - tmpspdr.Right_Front_Leg.Length) / 10;
    tmp[4] = (spdr->Right_Back_Leg.Length - tmpspdr.Right_Back_Leg.Length) / 10;
    tmp[5] = (spdr->Left_Middle_Leg.Length - tmpspdr.Left_Middle_Leg.Length) / 10;
    for(i=0; i<10; i++)
    {
        spdr->Right_Front_Leg.anglenear -= tmp[0];
        spdr->Right_Back_Leg.anglenear  -= tmp[1];
        spdr->Left_Middle_Leg.anglenear -= tmp[2];
        spdr->Right_Front_Leg.Length    -= tmp[3];
        spdr->Right_Back_Leg.Length     -= tmp[4];
        spdr->Left_Middle_Leg.Length    -= tmp[5];
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 1);
        cnt++;
    }
    tmpspdr = *spdr;
    /** A group legs drive the body forward */
    steplength = 0.0;
    for(i=0; i<5; i++)
    {
        steplength += 1.0;
        TRACK_StraightStepHorzontalA(spdr, steplength);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 1);
        cnt++;
    }
    /** B group legs down 5.0 centimeters */
    for(i=0; i<10; i++)
    {
        legheight += 5.0;
        TRACK_StraightStepVerticalB(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 1);
        cnt++;
    }
    /** A group legs up 5.0 centimeters and return to normal state */
    for(i=0; i<10; i++)
    {
        legheight -= 5.0;
        TRACK_StraightStepVerticalA(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 1);
        cnt++;
    }
    tmp[0] = (spdr->Left_Front_Leg.anglenear - tmpspdr.Left_Front_Leg.anglenear) / 10;
    tmp[1] = (spdr->Left_Back_Leg.anglenear - tmpspdr.Left_Back_Leg.anglenear) / 10;
    tmp[2] = (spdr->Right_Middle_Leg.anglenear - tmpspdr.Right_Middle_Leg.anglenear) / 10;
    tmp[3] = (spdr->Left_Front_Leg.Length - tmpspdr.Left_Front_Leg.Length) / 10;
    tmp[4] = (spdr->Left_Back_Leg.Length - tmpspdr.Left_Back_Leg.Length) / 10;
    tmp[5] = (spdr->Right_Middle_Leg.Length - tmpspdr.Right_Middle_Leg.Length) / 10;
    for(i=0; i<10; i++)
    {
        spdr->Left_Front_Leg.anglenear   -= tmp[0];
        spdr->Left_Back_Leg.anglenear    -= tmp[1];
        spdr->Right_Middle_Leg.anglenear -= tmp[2];
        spdr->Left_Front_Leg.Length      -= tmp[3];
        spdr->Left_Back_Leg.Length       -= tmp[4];
        spdr->Right_Middle_Leg.Length    -= tmp[5];
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 1);
        cnt++;
    }
    /** A group legs down 5.0 centimeters */
    for(i=0; i<10; i++)
    {
        legheight += 5.0;
        TRACK_StraightStepVerticalA(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 1);
        cnt++;
    }
}

/* */
void TRACK_Straight(SPIDER* spdr, TRACKBUFFER* trckbuf)
{
    u16 i = 0;
    for(i=0; i<STEPCOUNTER; i++)
    {
        SPDRRenovateAllData_S(spdr, trckbuf->JointPulse[i]);
        SPDRRenovateAllData_H(spdr);
        Delay(40);
    }
}


void TRACK_TurnLeftStepVerticalA(SPIDER* spdr, double legheight)
{
    spdr->Right_Middle_Leg.Height = legheight;
    spdr->Left_Front_Leg.Height = legheight;
    spdr->Left_Back_Leg.Height = legheight;
}

void TRACK_TurnLeftStepHorzontalA(SPIDER* spdr)
{
    /** Right middle leg */
    double oldlength = spdr->Right_Middle_Leg.Length;
    double oldangle  = spdr->Right_Middle_Leg.anglenear;
    double newlength = 0.0;
    double newangle  = 0.0;
    newlength = COMPUTEEDGE(PI/12, 65.0, 65.0+oldlength);
    newangle  = COMPUTEANGLE(65.0+oldlength, newlength, 65.0);
    newangle -= (PI / 2);
    newangle = (PI - newangle) * 180 / PI;
    spdr->Right_Middle_Leg.Length = newlength;
    spdr->Right_Middle_Leg.anglenear = newangle;

    /** Left front leg */
    oldlength = spdr->Left_Front_Leg.Length;
    oldangle  = spdr->Left_Front_Leg.anglenear;
    oldangle = oldangle * PI / 180;
    newlength = COMPUTEEDGE(PI/12, oldlength+85.0, 85.0);
    newangle = COMPUTEANGLE(oldlength+85.0, 85.0, newlength);
    newangle = PI - newangle;
    newangle = (oldangle + newangle) * 180 / PI;
    spdr->Left_Front_Leg.Length = newlength;
    spdr->Left_Front_Leg.anglenear = newangle;

    /** Left back leg */
    oldlength = spdr->Left_Back_Leg.Length;
    oldangle  = spdr->Left_Back_Leg.anglenear;
    oldangle = oldangle * PI / 180;
    newlength = COMPUTEEDGE(PI/12, 85.0, 85.0+oldlength);
    newangle =  COMPUTEANGLE(85.0+oldlength, 85.0, newlength);
    newangle = PI - newangle;
    newangle = (oldangle + newangle) * 180 / PI;
    spdr->Left_Back_Leg.Length = newlength;
    spdr->Left_Back_Leg.anglenear = newangle;
}

void TRACK_TurnLeftStepVerticalB(SPIDER* spdr, double legheight)
{
    spdr->Left_Middle_Leg.Height = legheight;
    spdr->Right_Front_Leg.Height = legheight;
    spdr->Right_Back_Leg.Height = legheight;
}

void TRACK_TurnLeftStepHorzontalB(SPIDER* spdr)
{
    /** Left middle leg */
    double oldlength = spdr->Left_Middle_Leg.Length;
    double oldangle  = spdr->Left_Middle_Leg.anglenear;
    double newlength = 0.0;
    double newangle = 0.0;

    newlength = COMPUTEEDGE(PI/12, 65.0, 65.0+oldlength);
    newangle  = COMPUTEANGLE(65.0+oldlength, newlength, 65.0);
    newangle -= (PI / 2);
    newangle = (PI - newangle) * 180 / PI;
    spdr->Left_Middle_Leg.Length = newlength;
    spdr->Left_Middle_Leg.anglenear = newangle;

    /** Right front leg */
    oldlength = spdr->Right_Front_Leg.Length;
    oldangle  = spdr->Right_Front_Leg.anglenear;
    oldangle = oldangle * PI / 180;
    newlength = COMPUTEEDGE(PI/12, oldlength+85.0, 85.0);
    newangle = COMPUTEANGLE(oldlength+85.0, 85.0, newlength);
    newangle = PI - newangle;
    newangle = (oldangle + newangle) * 180 / PI;
    spdr->Right_Front_Leg.Length = newlength;
    spdr->Right_Front_Leg.anglenear = newangle;

    /** Right back leg */
    oldlength = spdr->Right_Back_Leg.Length;
    oldangle  = spdr->Right_Back_Leg.anglenear;
    oldangle = oldangle * PI / 180;
    newlength = COMPUTEEDGE(PI/12, 85.0, 85.0+oldlength);
    newangle =  COMPUTEANGLE(85.0+oldlength, 85.0, newlength);
    newangle = PI - newangle;
    newangle = (oldangle + newangle) * 180 / PI;
    spdr->Right_Back_Leg.Length = newlength;
    spdr->Right_Back_Leg.anglenear = newangle;
}

void TRACK_TurnLeftStep(SPIDER* spdr, SPIDER* original, TRACKBUFFER* trckbuf)
{
    double legheight = spdr->Spdr_Height;
    double tmp[6] = {0};
	u16 i = 0;
	u16 cnt = 0;
    /** A group legs up 5.0 centimeters */
    for(i=0; i<10; i++)
    {
        legheight -= 5.0;
        TRACK_TurnLeftStepVerticalA(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 3);
        cnt++;
    }
    /*
    tmp[0] = (spdr->Left_Front_Leg.anglenear - original->Left_Front_Leg.anglenear) / 10;
    tmp[1] = (spdr->Left_Back_Leg.anglenear - original->Left_Back_Leg.anglenear) / 10;
    tmp[2] = (spdr->Right_Middle_Leg.anglenear - original->Right_Middle_Leg.anglenear) / 10;
    tmp[3] = (spdr->Left_Front_Leg.Length - original->Left_Front_Leg.Length) / 10;
    tmp[4] = (spdr->Left_Back_Leg.Length - original->Left_Back_Leg.Length) / 10;
    tmp[5] = (spdr->Right_Middle_Leg.Length - original->Right_Middle_Leg.Length) / 10;
    for(i=0; i<10; i++)
    {
        spdr->Left_Front_Leg.anglenear   -= tmp[0];
        spdr->Left_Back_Leg.anglenear    -= tmp[1];
        spdr->Right_Middle_Leg.anglenear -= tmp[2];
        spdr->Left_Front_Leg.Length      -= tmp[3];
        spdr->Left_Back_Leg.Length       -= tmp[4];
        spdr->Right_Middle_Leg.Length    -= tmp[5];
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 3);
        cnt++;
    }*/

    /** B group legs drive the body turn left 30 degree */
    TRACK_TurnLeftStepHorzontalB(spdr);
    tmp[0] = (spdr->Right_Front_Leg.anglenear - original->Right_Front_Leg.anglenear) / 10;
    tmp[1] = (spdr->Right_Back_Leg.anglenear - original->Right_Back_Leg.anglenear) / 10;
    tmp[2] = (spdr->Left_Middle_Leg.anglenear - original->Left_Middle_Leg.anglenear) / 10;
    tmp[3] = (spdr->Right_Front_Leg.Length - original->Right_Front_Leg.Length) / 10;
    tmp[4] = (spdr->Right_Back_Leg.Length - original->Right_Back_Leg.Length) / 10;
    tmp[5] = (spdr->Left_Middle_Leg.Length - original->Left_Middle_Leg.Length) / 10;
    spdr->Right_Front_Leg.anglenear = original->Right_Front_Leg.anglenear;
    spdr->Right_Back_Leg.anglenear = original->Right_Back_Leg.anglenear;
    spdr->Left_Middle_Leg.anglenear = original->Left_Middle_Leg.anglenear;
    spdr->Right_Front_Leg.Length = original->Right_Front_Leg.Length;
    spdr->Right_Back_Leg.Length = original->Right_Back_Leg.Length;
    spdr->Left_Middle_Leg.Length = original->Left_Middle_Leg.Length;
    for(i=0; i<10; i++)
    {
        spdr->Right_Front_Leg.anglenear += tmp[0];
        spdr->Right_Back_Leg.anglenear  += tmp[1];
        spdr->Left_Middle_Leg.anglenear += tmp[2];
        spdr->Right_Front_Leg.Length    += tmp[3];
        spdr->Right_Back_Leg.Length     += tmp[4];
        spdr->Left_Middle_Leg.Length    += tmp[5];
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 3);
        cnt++;
    }

    /** A group legs down 5.0 centimeters */
    for(i=0; i<10; i++)
    {
        legheight += 5.0;
        TRACK_TurnLeftStepVerticalA(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 3);
        cnt++;
    }
    /** B group legs up 5.0 centimeters and return to normal state */
    for(i=0; i<10; i++)
    {
        legheight -= 5.0;
        TRACK_TurnLeftStepVerticalB(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 3);
        cnt++;
    }

    tmp[0] = (spdr->Right_Front_Leg.anglenear - original->Right_Front_Leg.anglenear) / 10;
    tmp[1] = (spdr->Right_Back_Leg.anglenear - original->Right_Back_Leg.anglenear) / 10;
    tmp[2] = (spdr->Left_Middle_Leg.anglenear - original->Left_Middle_Leg.anglenear) / 10;
    tmp[3] = (spdr->Right_Front_Leg.Length - original->Right_Front_Leg.Length) / 10;
    tmp[4] = (spdr->Right_Back_Leg.Length - original->Right_Back_Leg.Length) / 10;
    tmp[5] = (spdr->Left_Middle_Leg.Length - original->Left_Middle_Leg.Length) / 10;
    for(i=0; i<10; i++)
    {
        spdr->Right_Front_Leg.anglenear -= tmp[0];
        spdr->Right_Back_Leg.anglenear  -= tmp[1];
        spdr->Left_Middle_Leg.anglenear -= tmp[2];
        spdr->Right_Front_Leg.Length    -= tmp[3];
        spdr->Right_Back_Leg.Length     -= tmp[4];
        spdr->Left_Middle_Leg.Length    -= tmp[5];
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 3);
        cnt++;
    }

    /** A group legs drive the body turn left 30 degree */
    TRACK_TurnLeftStepHorzontalA(spdr);
    tmp[0] = (spdr->Left_Front_Leg.anglenear - original->Left_Front_Leg.anglenear) / 10;
    tmp[1] = (spdr->Left_Back_Leg.anglenear - original->Left_Back_Leg.anglenear) / 10;
    tmp[2] = (spdr->Right_Middle_Leg.anglenear - original->Right_Middle_Leg.anglenear) / 10;
    tmp[3] = (spdr->Left_Front_Leg.Length - original->Left_Front_Leg.Length) / 10;
    tmp[4] = (spdr->Left_Back_Leg.Length - original->Left_Back_Leg.Length) / 10;
    tmp[5] = (spdr->Right_Middle_Leg.Length - original->Right_Middle_Leg.Length) / 10;
    spdr->Left_Front_Leg.anglenear = original->Left_Front_Leg.anglenear;
    spdr->Left_Back_Leg.anglenear = original->Left_Back_Leg.anglenear;
    spdr->Right_Middle_Leg.anglenear = original->Right_Middle_Leg.anglenear;
    spdr->Left_Front_Leg.Length = original->Left_Front_Leg.Length;
    spdr->Left_Back_Leg.Length = original->Left_Back_Leg.Length;
    spdr->Right_Middle_Leg.Length = original->Right_Middle_Leg.Length;
    for(i=0; i<10; i++)
    {
        spdr->Left_Front_Leg.anglenear    += tmp[0];
        spdr->Left_Back_Leg.anglenear     += tmp[1];
        spdr->Right_Middle_Leg.anglenear  += tmp[2];
        spdr->Left_Front_Leg.Length       += tmp[3];
        spdr->Left_Back_Leg.Length        += tmp[4];
        spdr->Right_Middle_Leg.Length     += tmp[5];
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 3);
        cnt++;
    }

    /** B group legs down 5.0 centimeters */
    for(i=0; i<10; i++)
    {
        legheight += 5.0;
        TRACK_TurnLeftStepVerticalB(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 3);
        cnt++;
    }
    /** A group legs up 5.0 centimeters and return to normal state */
    for(i=0; i<10; i++)
    {
        legheight -= 5.0;
        TRACK_TurnLeftStepVerticalA(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 3);
        cnt++;
    }
    tmp[0] = (spdr->Left_Front_Leg.anglenear - original->Left_Front_Leg.anglenear) / 10;
    tmp[1] = (spdr->Left_Back_Leg.anglenear - original->Left_Back_Leg.anglenear) / 10;
    tmp[2] = (spdr->Right_Middle_Leg.anglenear - original->Right_Middle_Leg.anglenear) / 10;
    tmp[3] = (spdr->Left_Front_Leg.Length - original->Left_Front_Leg.Length) / 10;
    tmp[4] = (spdr->Left_Back_Leg.Length - original->Left_Back_Leg.Length) / 10;
    tmp[5] = (spdr->Right_Middle_Leg.Length - original->Right_Middle_Leg.Length) / 10;
    for(i=0; i<10; i++)
    {
        spdr->Left_Front_Leg.anglenear   -= tmp[0];
        spdr->Left_Back_Leg.anglenear    -= tmp[1];
        spdr->Right_Middle_Leg.anglenear -= tmp[2];
        spdr->Left_Front_Leg.Length      -= tmp[3];
        spdr->Left_Back_Leg.Length       -= tmp[4];
        spdr->Right_Middle_Leg.Length    -= tmp[5];
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 3);
        cnt++;
    }
    /** A group legs down 5.0 centimeters */
    for(i=0; i<10; i++)
    {
        legheight += 5.0;
        TRACK_TurnLeftStepVerticalA(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 3);
        cnt++;
    }
}

void TRACK_TurnLeft(SPIDER* spdr, TRACKBUFFER* trckbuf)
{
    u16 i = 0;
    for(i=0; i<STEPLEFTCNT; i++)
    {
        SPDRRenovateAllData_S(spdr, trckbuf->JointPulseTL[i]);
        SPDRRenovateAllData_H(spdr);
        Delay(50);
    }
}



void TRACK_TurnRight(SPIDER* spdr, TRACKBUFFER* trckbuf)
{
    u16 i = 0;
    for(i=0; i<STEPRIGHTCNT; i++)
    {
        SPDRRenovateAllData_S(spdr, trckbuf->JointPulseTR[i]);
        SPDRRenovateAllData_H(spdr);
        Delay(50);
    }
}

void TRACK_TurnRightStep(SPIDER* spdr, SPIDER* original, TRACKBUFFER* trckbuf)
{
    double legheight = spdr->Spdr_Height;
    double tmp[6] = {0};
	u16 i = 0;
	u16 cnt = 0;
    /** A group legs up 5.0 centimeters and return to normal state */
    for(i=0; i<10; i++)
    {
        legheight -= 5.0;
        TRACK_TurnRightStepVerticalA(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 4);
        cnt++;
    }
    /*
    tmp[0] = (spdr->Left_Front_Leg.anglenear - original->Left_Front_Leg.anglenear) / 10;
    tmp[1] = (spdr->Left_Back_Leg.anglenear - original->Left_Back_Leg.anglenear) / 10;
    tmp[2] = (spdr->Right_Middle_Leg.anglenear - original->Right_Middle_Leg.anglenear) / 10;
    tmp[3] = (spdr->Left_Front_Leg.Length - original->Left_Front_Leg.Length) / 10;
    tmp[4] = (spdr->Left_Back_Leg.Length - original->Left_Back_Leg.Length) / 10;
    tmp[5] = (spdr->Right_Middle_Leg.Length - original->Right_Middle_Leg.Length) / 10;
    for(i=0; i<10; i++)
    {
        spdr->Left_Front_Leg.anglenear   -= tmp[0];
        spdr->Left_Back_Leg.anglenear    -= tmp[1];
        spdr->Right_Middle_Leg.anglenear -= tmp[2];
        spdr->Left_Front_Leg.Length      -= tmp[3];
        spdr->Left_Back_Leg.Length       -= tmp[4];
        spdr->Right_Middle_Leg.Length    -= tmp[5];
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 4);
        cnt++;
    }
    */

    /** B group legs drive the body turn right 30 degree */
    TRACK_TurnRightStepHorzontalB(spdr);
    tmp[0] = (spdr->Right_Front_Leg.anglenear - original->Right_Front_Leg.anglenear) / 10;
    tmp[1] = (spdr->Right_Back_Leg.anglenear - original->Right_Back_Leg.anglenear) / 10;
    tmp[2] = (spdr->Left_Middle_Leg.anglenear - original->Left_Middle_Leg.anglenear) / 10;
    tmp[3] = (spdr->Right_Front_Leg.Length - original->Right_Front_Leg.Length) / 10;
    tmp[4] = (spdr->Right_Back_Leg.Length - original->Right_Back_Leg.Length) / 10;
    tmp[5] = (spdr->Left_Middle_Leg.Length - original->Left_Middle_Leg.Length) / 10;
    spdr->Right_Front_Leg.anglenear = original->Right_Front_Leg.anglenear;
    spdr->Right_Back_Leg.anglenear = original->Right_Back_Leg.anglenear;
    spdr->Left_Middle_Leg.anglenear = original->Left_Middle_Leg.anglenear;
    spdr->Right_Front_Leg.Length = original->Right_Front_Leg.Length;
    spdr->Right_Back_Leg.Length = original->Right_Back_Leg.Length;
    spdr->Left_Middle_Leg.Length = original->Left_Middle_Leg.Length;
    for(i=0; i<10; i++)
    {
        spdr->Right_Front_Leg.anglenear += tmp[0];
        spdr->Right_Back_Leg.anglenear  += tmp[1];
        spdr->Left_Middle_Leg.anglenear += tmp[2];
        spdr->Right_Front_Leg.Length    += tmp[3];
        spdr->Right_Back_Leg.Length     += tmp[4];
        spdr->Left_Middle_Leg.Length    += tmp[5];
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 4);
        cnt++;
    }

    /** A group legs down 5.0 centimeters */
    for(i=0; i<10; i++)
    {
        legheight += 5.0;
        TRACK_TurnRightStepVerticalA(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 4);
        cnt++;
    }
    /** B group legs up 5.0 centimeters and return to normal state */
    for(i=0; i<10; i++)
    {
        legheight -= 5.0;
        TRACK_TurnRightStepVerticalB(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 4);
        cnt++;
    }

    tmp[0] = (spdr->Right_Front_Leg.anglenear - original->Right_Front_Leg.anglenear) / 10;
    tmp[1] = (spdr->Right_Back_Leg.anglenear - original->Right_Back_Leg.anglenear) / 10;
    tmp[2] = (spdr->Left_Middle_Leg.anglenear - original->Left_Middle_Leg.anglenear) / 10;
    tmp[3] = (spdr->Right_Front_Leg.Length - original->Right_Front_Leg.Length) / 10;
    tmp[4] = (spdr->Right_Back_Leg.Length - original->Right_Back_Leg.Length) / 10;
    tmp[5] = (spdr->Left_Middle_Leg.Length - original->Left_Middle_Leg.Length) / 10;
    for(i=0; i<10; i++)
    {
        spdr->Right_Front_Leg.anglenear -= tmp[0];
        spdr->Right_Back_Leg.anglenear  -= tmp[1];
        spdr->Left_Middle_Leg.anglenear -= tmp[2];
        spdr->Right_Front_Leg.Length    -= tmp[3];
        spdr->Right_Back_Leg.Length     -= tmp[4];
        spdr->Left_Middle_Leg.Length    -= tmp[5];
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 4);
        cnt++;
    }

    /** A group legs drive the body turn right 30 degree */
    TRACK_TurnRightStepHorzontalA(spdr);
    tmp[0] = (spdr->Left_Front_Leg.anglenear - original->Left_Front_Leg.anglenear) / 10;
    tmp[1] = (spdr->Left_Back_Leg.anglenear - original->Left_Back_Leg.anglenear) / 10;
    tmp[2] = (spdr->Right_Middle_Leg.anglenear - original->Right_Middle_Leg.anglenear) / 10;
    tmp[3] = (spdr->Left_Front_Leg.Length - original->Left_Front_Leg.Length) / 10;
    tmp[4] = (spdr->Left_Back_Leg.Length - original->Left_Back_Leg.Length) / 10;
    tmp[5] = (spdr->Right_Middle_Leg.Length - original->Right_Middle_Leg.Length) / 10;
    spdr->Left_Front_Leg.anglenear = original->Left_Front_Leg.anglenear;
    spdr->Left_Back_Leg.anglenear = original->Left_Back_Leg.anglenear;
    spdr->Right_Middle_Leg.anglenear = original->Right_Middle_Leg.anglenear;
    spdr->Left_Front_Leg.Length = original->Left_Front_Leg.Length;
    spdr->Left_Back_Leg.Length = original->Left_Back_Leg.Length;
    spdr->Right_Middle_Leg.Length = original->Right_Middle_Leg.Length;
    for(i=0; i<10; i++)
    {
        spdr->Left_Front_Leg.anglenear    += tmp[0];
        spdr->Left_Back_Leg.anglenear     += tmp[1];
        spdr->Right_Middle_Leg.anglenear  += tmp[2];
        spdr->Left_Front_Leg.Length       += tmp[3];
        spdr->Left_Back_Leg.Length        += tmp[4];
        spdr->Right_Middle_Leg.Length     += tmp[5];
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 4);
        cnt++;
    }
    /** B group legs down 5.0 centimeters */
    for(i=0; i<10; i++)
    {
        legheight += 5.0;
        TRACK_TurnRightStepVerticalB(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 4);
        cnt++;
    }
    /** A group legs up 5.0 centimeters and return to normal state */
    for(i=0; i<10; i++)
    {
        legheight -= 5.0;
        TRACK_TurnRightStepVerticalA(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 4);
        cnt++;
    }
    tmp[0] = (spdr->Left_Front_Leg.anglenear - original->Left_Front_Leg.anglenear) / 10;
    tmp[1] = (spdr->Left_Back_Leg.anglenear - original->Left_Back_Leg.anglenear) / 10;
    tmp[2] = (spdr->Right_Middle_Leg.anglenear - original->Right_Middle_Leg.anglenear) / 10;
    tmp[3] = (spdr->Left_Front_Leg.Length - original->Left_Front_Leg.Length) / 10;
    tmp[4] = (spdr->Left_Back_Leg.Length - original->Left_Back_Leg.Length) / 10;
    tmp[5] = (spdr->Right_Middle_Leg.Length - original->Right_Middle_Leg.Length) / 10;
    for(i=0; i<10; i++)
    {
        spdr->Left_Front_Leg.anglenear   -= tmp[0];
        spdr->Left_Back_Leg.anglenear    -= tmp[1];
        spdr->Right_Middle_Leg.anglenear -= tmp[2];
        spdr->Left_Front_Leg.Length      -= tmp[3];
        spdr->Left_Back_Leg.Length       -= tmp[4];
        spdr->Right_Middle_Leg.Length    -= tmp[5];
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 4);
        cnt++;
    }
    /** A group legs down 5.0 centimeters */
    for(i=0; i<10; i++)
    {
        legheight += 5.0;
        TRACK_TurnRightStepVerticalA(spdr, legheight);
        TRACK_CreateAngle(spdr, &(trckbuf->Trck));
        TRACK_TransAngleToPulse(trckbuf, cnt, 4);
        cnt++;
    }

}

void TRACK_TurnRightStepVerticalA(SPIDER* spdr, double legheight)
{
    spdr->Right_Middle_Leg.Height = legheight;
    spdr->Left_Front_Leg.Height = legheight;
    spdr->Left_Back_Leg.Height = legheight;
}

void TRACK_TurnRightStepHorzontalA(SPIDER* spdr)
{
    /** Right middle leg */
    double oldlength = spdr->Right_Middle_Leg.Length;
    double oldangle  = spdr->Right_Middle_Leg.anglenear;
    double newlength = 0.0;
    double newangle  = 0.0;
    newlength = COMPUTEEDGE(PI/12, 65.0, 65.0+oldlength);
    newangle  = COMPUTEANGLE(65.0+oldlength, newlength, 65.0);
    newangle -= (PI / 2);
    newangle =  newangle * 180 / PI;
    spdr->Right_Middle_Leg.Length = newlength;
    spdr->Right_Middle_Leg.anglenear = newangle;

    /** Left front leg */
    oldlength = spdr->Left_Front_Leg.Length;
    oldangle  = spdr->Left_Front_Leg.anglenear;
    oldangle = oldangle * PI / 180;
    newlength = COMPUTEEDGE(PI/12, oldlength+85.0, 85.0);
    newangle = COMPUTEANGLE(oldlength+85.0, 85.0, newlength);
    newangle = PI - newangle;
    newangle = (oldangle - newangle) * 180 / PI;
    spdr->Left_Front_Leg.Length = newlength;
    spdr->Left_Front_Leg.anglenear = newangle;

    /** Left back leg */
    oldlength = spdr->Left_Back_Leg.Length;
    oldangle  = spdr->Left_Back_Leg.anglenear;
    oldangle = oldangle * PI / 180;
    newlength = COMPUTEEDGE(PI/12, 85.0, 85.0+oldlength);
    newangle =  COMPUTEANGLE(85.0+oldlength, 85.0, newlength);
    newangle = PI - newangle;
    newangle = (oldangle - newangle) * 180 / PI;
    spdr->Left_Back_Leg.Length = newlength;
    spdr->Left_Back_Leg.anglenear = newangle;

}

void TRACK_TurnRightStepVerticalB(SPIDER* spdr, double legheight)
{
    spdr->Left_Middle_Leg.Height = legheight;
    spdr->Right_Front_Leg.Height = legheight;
    spdr->Right_Back_Leg.Height = legheight;
}
void TRACK_TurnRightStepHorzontalB(SPIDER* spdr)
{
    /** Left middle leg */
    double oldlength = spdr->Left_Middle_Leg.Length;
    double oldangle  = spdr->Left_Middle_Leg.anglenear;
    double newlength = 0.0;
    double newangle = 0.0;

    newlength = COMPUTEEDGE(PI/12, 65.0, 65.0+oldlength);
    newangle  = COMPUTEANGLE(65.0+oldlength, newlength, 65.0);
    newangle -= (PI / 2);
    newangle = newangle * 180 / PI;
    spdr->Left_Middle_Leg.Length = newlength;
    spdr->Left_Middle_Leg.anglenear = newangle;

    /** Right front leg */
    oldlength = spdr->Right_Front_Leg.Length;
    oldangle  = spdr->Right_Front_Leg.anglenear;
    oldangle = oldangle * PI / 180;
    newlength = COMPUTEEDGE(PI/12, oldlength+85.0, 85.0);
    newangle = COMPUTEANGLE(oldlength+85.0, 85.0, newlength);
    newangle = PI - newangle;
    newangle = (oldangle - newangle) * 180 / PI;
    spdr->Right_Front_Leg.Length = newlength;
    spdr->Right_Front_Leg.anglenear = newangle;

    /** Right back leg */
    oldlength = spdr->Right_Back_Leg.Length;
    oldangle  = spdr->Right_Back_Leg.anglenear;
    oldangle = oldangle * PI / 180;
    newlength = COMPUTEEDGE(PI/12, 85.0, 85.0+oldlength);
    newangle =  COMPUTEANGLE(85.0+oldlength, 85.0, newlength);
    newangle = PI - newangle;
    newangle = (oldangle - newangle) * 180 / PI;
    spdr->Right_Back_Leg.Length = newlength;
    spdr->Right_Back_Leg.anglenear = newangle;

}

六足步态程序spidertrack.h__________点击查看代码

#ifndef __SPIDERTRACK_H
#define __SPIDERTRACK_H

#include "stm32f10x_lib.h"
#include "spider.h"
/** Define constants and some factors
    used for transform */
#define PI 3.14156

/** STEPCOUNTER must in the range of U16MAX */
#define STEPCOUNTER  90
#define STEPLEFTCNT  100
#define STEPRIGHTCNT 100
/** */

typedef struct
{
    double jointangle[18];
}TRACK;

typedef struct
{
    TRACK Trck;
    u16   JointPulse[STEPCOUNTER][18];
    u16   JointPulseTLP[10][18];
    u16   JointPulseTL[STEPLEFTCNT][18];
    u16   JointPulseTR[STEPRIGHTCNT][18];
}TRACKBUFFER;


/** return in angle */
void TRACK_LengthToAngle(double height, double length, double* anglefar, double* anglemid);
void TRACK_CreateAngle(SPIDER* spdr, TRACK* trck);
void TRACK_TransAngleToPulse(TRACKBUFFER* trckbuf,u16 cnt,u8 type);

void TRACK_Straight(SPIDER* spdr, TRACKBUFFER* trckbuf);
void TRACK_StraightStep(SPIDER* spdr, SPIDER* original, TRACKBUFFER* trckbuf);
void TRACK_StraightStepVerticalA(SPIDER* spdr, double legheight);
void TRACK_StraightStepHorzontalA(SPIDER* spdr, double steplength);
void TRACK_StraightStepVerticalB(SPIDER* spdr, double legheight);
void TRACK_StraightStepHorzontalB(SPIDER* spdr, double steplength);

void TRACK_TurnLeft(SPIDER* spdr, TRACKBUFFER* trckbuf);
void TRACK_TurnLeftStep(SPIDER* spdr, SPIDER* original, TRACKBUFFER* trckbuf);
void TRACK_TurnLeftStepVerticalA(SPIDER* spdr, double legheight);
void TRACK_TurnLeftStepHorzontalA(SPIDER* spdr);
void TRACK_TurnLeftStepVerticalB(SPIDER* spdr, double legheight);
void TRACK_TurnLeftStepHorzontalB(SPIDER* spdr);

void TRACK_TurnRight(SPIDER* spdr, TRACKBUFFER* trckbuf);
void TRACK_TurnRightStep(SPIDER* spdr, SPIDER* original, TRACKBUFFER* trckbuf);
void TRACK_TurnRightStepVerticalA(SPIDER* spdr, double legheight);
void TRACK_TurnRightStepHorzontalA(SPIDER* spdr);
void TRACK_TurnRightStepVerticalB(SPIDER* spdr, double legheight);
void TRACK_TurnRightStepHorzontalB(SPIDER* spdr);
#endif /* __SPIDERTRACK_H */

4. 外设数据初始化stm32f10x_init.c 和 stm32f10x_init.h

外设数据初始化stm32f10x_init.c__________点击查看代码

/* Includes ------------------------------------------------------------------*/
#include "stm32f10x_init.h"

/* Private typedef -----------------------------------------------------------*/
typedef enum { FAILED = 0, PASSED = !FAILED} TestStatus;

/* Private define ------------------------------------------------------------*/
#define TIM8_HON_OFFSET ((u16)(0 * 4))
#define TIM8_VER_OFFSET ((u16)(7 * 4))
#define RFLJF_OFFSET    ((u16)(0 * 4))
#define RFLJM_OFFSET    ((u16)(6 * 4))
#define RFLJN_OFFSET    ((u16)(3 * 4))
#define RMLJF_OFFSET    ((u16)(15 * 4))
#define RMLJM_OFFSET    ((u16)(4 * 4))
#define RMLJN_OFFSET    ((u16)(10 * 4))
#define RBLJF_OFFSET    ((u16)(0 * 4))
#define RBLJM_OFFSET    ((u16)(0 * 4))
#define RBLJN_OFFSET    ((u16)(0 * 4))
#define LFLJF_OFFSET    ((u16)(0 * 4))
#define LFLJM_OFFSET    ((u16)(0 * 4))
#define LFLJN_OFFSET    ((u16)(3 * 4))
#define LMLJF_OFFSET    ((u16)(0 * 4))
#define LMLJM_OFFSET    ((u16)(0 * 4))
#define LMLJN_OFFSET    ((u16)(8 * 4))
#define LBLJF_OFFSET    ((u16)(0 * 4))
#define LBLJM_OFFSET    ((u16)(0 * 4))
#define LBLJN_OFFSET    ((u16)(0 * 4))


/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
GPIO_InitTypeDef            GPIO_InitStructure;
NVIC_InitTypeDef            NVIC_InitStructure;
USART_InitTypeDef           USART_InitStructure;
TIM_OCInitTypeDef           TIM_OCInitStructure;
TIM_TimeBaseInitTypeDef     TIM_TimeBaseStructure;
ErrorStatus                 HSEStartUpStatus;

static vu32 TimingDelay = 0;
static SOFTTIMER SoftTimer = {0};


/********************************************************
* Function Name : Stm32f10x_Configuration
* Description   : Configure the system clock
                  peripheral drivers GPIO EXTI TIM SYSTIC
                  ADC and so on
* Input         : None
* Output        : None
* Return        : None
*********************************************************/
void Stm32f10x_Configuration(SPIDER* spdr)
{
    RCC_Configuration();
    GPIO_Configuration();
    SysTick_Configuration();
    USART1_Configuration();
    USART3_Configuration();
    TIM1_Configuration(spdr);
    TIM2_Configuration(spdr);
    TIM3_Configuration(spdr);
    TIM4_Configuration(spdr);
    TIM5_Configuration(spdr);
}

/********************************************************
* Function Name : Stm32f10x_Initialization
* Description   : Enable the interrupts such as the Systick
                  for the Delay function.
* Input         : None
* Output        : None
* Return        : None
*********************************************************/
void Stm32f10x_Initialization(void)
{
    /* Enable the SysTick Counter */
    SysTick_CounterCmd(SysTick_Counter_Enable);

}

/********************************************************
* Function Name : RCC_Configuration
* Description   : Configure the system clock
                  SYSCLK = PLLCLK = 72 MHz
                  (AHB) HCLK      = 72 MHz
                  (APB1)PCLK1     = 36 MHz
                  (APB2)PCLK2     = 72 MHz
* Input         : None
* Output        : None
* Return        : None
*********************************************************/
void RCC_Configuration(void)
{
    RCC_DeInit();
    // Enable HSE
    RCC_HSEConfig(RCC_HSE_ON);
    HSEStartUpStatus = RCC_WaitForHSEStartUp();
    if(HSEStartUpStatus == SUCCESS)
    {
        RCC_HCLKConfig(RCC_SYSCLK_Div1);
        RCC_PCLK2Config(RCC_HCLK_Div1);
        RCC_PCLK1Config(RCC_HCLK_Div2);
        // Enable flash prefetch buffer
        FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
        FLASH_SetLatency(FLASH_Latency_2);
        // PLLCLK = 72MHz
        RCC_PLLConfig(RCC_PLLSource_HSE_Div1, RCC_PLLMul_9);
        RCC_PLLCmd(ENABLE);
        while(RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET)
        {
        }
        RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
        while(RCC_GetSYSCLKSource() != 0x08)
        {
        }
    }
    /* AFIO GPIOB GPIOC and GPIOD clock enable */
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB |
                           RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD |
                           RCC_APB2Periph_GPIOE | RCC_APB2Periph_GPIOG |
                           RCC_APB2Periph_AFIO, ENABLE);
    /* USART1 clock enable */
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1 , ENABLE);
    /* USART3 clock enable */
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
    /* TIM1-5 clock enable */
    /** Attention: the orignal code set the APB1 CLK = HCLK_Div4
        here we set the APB1 CLK = HCLK_Div2 ,so the TIM2-5 clk
        is twice fast than orignal */
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
    /** TIM8 used for the camera */
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);

}



void GPIO_Configuration(void)
{
    /** GPIOE 9,11,13,14 Configuration: TIM1 channel 1, 2, 3 and 4 */
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_11 | GPIO_Pin_13 | GPIO_Pin_14;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOE, &GPIO_InitStructure);
    GPIO_PinRemapConfig(GPIO_FullRemap_TIM1, ENABLE);

    /** GPIOB 10,11 Configuration: TIM2 channel 3 and 4 */
	GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_10 | GPIO_Pin_11;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	GPIO_PinRemapConfig(GPIO_PartialRemap2_TIM2, ENABLE);

 	/** GPIOC 6,7,8,9 Configuration: TIM8 channel 1, 2, 3 and 4*/
 	GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9;
 	GPIO_Init(GPIOC, &GPIO_InitStructure);


 	/* GPIOB 0,1,4,5 Configuration: TIM3 channel 1,2,3 and 4
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_4 | GPIO_Pin_5;
 	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
 	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
 	GPIO_Init(GPIOB, &GPIO_InitStructure);
 	GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3, ENABLE);*/

 	/** No remap for TIM3, GPIOA 6,7 and GPIOB 0,1 for TIM3 channel 1, 2, 3 and 4 */
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
 	GPIO_Init(GPIOA, &GPIO_InitStructure);
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
 	GPIO_Init(GPIOB, &GPIO_InitStructure);

	/** GPIOD 12,13,14,15 Configuration: TIM4 channel 1, 2, 3 and 4 */
	GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
	GPIO_Init(GPIOD, &GPIO_InitStructure);
	GPIO_PinRemapConfig(GPIO_Remap_TIM4, ENABLE);

	/** GPIOA 0,1,2,3 Configuration: TIM5 Channel 1, 2, 3 and 4 */
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

    /** GPIOC 6,7 Configuration: TIM8 channel 1, 2 */
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
    GPIO_Init(GPIOC, &GPIO_InitStructure);

    /** GPIOA Configuration: USART1_TX (PB06) USART_RX (PB07)
        used for GPS */
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOB, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOB, &GPIO_InitStructure);

    GPIO_PinRemapConfig(GPIO_Remap_USART1, ENABLE);

    /** GPIOD Configuration: USART3_TX (PD08) USART3_RX (PD09)
        used to connect to the PC */
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOD, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOD, &GPIO_InitStructure);

    GPIO_PinRemapConfig(GPIO_FullRemap_USART3, ENABLE);
}



/* Connected to the Zigbee modules */
void USART1_Configuration(void)
{
    USART_InitStructure.USART_BaudRate = 9600;
    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
    USART_InitStructure.USART_StopBits = USART_StopBits_1;
    USART_InitStructure.USART_Parity = USART_Parity_No;
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
    USART_Init(USART1, &USART_InitStructure);

    //USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
    //USART_ITConfig(USART1, USART_IT_TXE, ENABLE);

    USART_Cmd(USART1, ENABLE);


    NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQChannel;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);
}

void USART3_Configuration(void)
{
    USART_InitStructure.USART_BaudRate = 4800;
    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
    USART_InitStructure.USART_StopBits = USART_StopBits_1;
    USART_InitStructure.USART_Parity = USART_Parity_No;
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
    USART_Init(USART3, &USART_InitStructure);

    //USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
    //USART_ITConfig(USART3, USART_IT_TXE, ENABLE);

    USART_Cmd(USART3, ENABLE);
    NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQChannel;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);
}


void SysTick_Configuration(void)
{
    SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK_Div8);
    /* SysTick end of count event each 1ms with input clock equal to 9MHz (HCLK/8, default) */
    SysTick_SetReload(9000);
    /* Enable SysTick interrupt */
    SysTick_ITConfig(ENABLE);
}
/*******************************************************************************
* Function Name  : Delay
* Description    : Inserts a delay time.
* Input          : nTime: specifies the delay time length, in milliseconds.
* Output         : None
* Return         : None
*******************************************************************************/
void Delay(u32 nTime)
{

  TimingDelay = nTime;

  while(TimingDelay != 0);

}

void TimingDelay_Decrement(void)
{
  if (TimingDelay != 0x00)
  {
    TimingDelay--;
  }
  if(SoftTimer.start == 1)
  {
      SoftTimer.nTime--;
      if(SoftTimer.nTime == 0)
      {
          SoftTimer.start = 0;
          SoftTimer.flag = 1;
      }
  }
}
void SFTMRStart(u32 nTime)
{
    SoftTimer.nTime = nTime;
    SoftTimer.start = 1;
}

u8  SFTMRCheck(void)
{
    return SoftTimer.flag;
}

/** TIM8 used for the camera */
void TIM8_Configuration(CAMERA* came)
{
    // Count CLK freq = 36000,Period = 7200,ie 20ms.
    TIM_TimeBaseStructure.TIM_Period = 7200;
    TIM_TimeBaseStructure.TIM_Prescaler = 200;
    TIM_TimeBaseStructure.TIM_ClockDivision = 0;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure);
    /** TIM8 CH1 */
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = came->Horizontal + TIM8_HON_OFFSET;
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OC1Init(TIM8, &TIM_OCInitStructure);
    TIM_OC1PreloadConfig(TIM8, TIM_OCPreload_Enable);
    /** TIM8 CH2 */
    TIM_OCInitStructure.TIM_Pulse = came->Vertical + TIM8_VER_OFFSET;
    TIM_OC2Init(TIM8, &TIM_OCInitStructure);
    TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Enable);

	TIM_ARRPreloadConfig(TIM8, ENABLE);
	/** TIM8 enable counter */
    TIM_Cmd(TIM8, ENABLE);
    /** TIM8 must has the following instruction */
    TIM_CtrlPWMOutputs(TIM8, ENABLE);
}

void TIM8_HorizontalRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + TIM8_HON_OFFSET;
    TIM_OC1Init(TIM8, &TIM_OCInitStructure);
    TIM_OC1PreloadConfig(TIM8, TIM_OCPreload_Enable);
}

void TIM8_VerticalRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + TIM8_VER_OFFSET;
    TIM_OC2Init(TIM8, &TIM_OCInitStructure);
    TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Enable);
}

void TIM1_Configuration(SPIDER* spdr)
{
    // Count CLK freq = 36000,Period = 7200,ie 20ms.
    TIM_TimeBaseStructure.TIM_Period = 7200;
    TIM_TimeBaseStructure.TIM_Prescaler = 200;
    TIM_TimeBaseStructure.TIM_ClockDivision = 0;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
    /** TIM1 CH1 */
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = spdr->Right_Front_Leg.Joint_Far + RFLJF_OFFSET;
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OC1Init(TIM1, &TIM_OCInitStructure);
    TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);
    /** TIM1 CH2 */
    TIM_OCInitStructure.TIM_Pulse = spdr->Right_Front_Leg.Joint_Mid + RFLJM_OFFSET;
    TIM_OC2Init(TIM1, &TIM_OCInitStructure);
    TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable);
    /** TIM1 CH3 */
    TIM_OCInitStructure.TIM_Pulse = spdr->Right_Front_Leg.Joint_Near + RFLJN_OFFSET;
    TIM_OC3Init(TIM1, &TIM_OCInitStructure);
    TIM_OC3PreloadConfig(TIM1, TIM_OCPreload_Enable);
    /** TIM1 CH4 */
    TIM_OCInitStructure.TIM_Pulse = spdr->Right_Middle_Leg.Joint_Far - RMLJF_OFFSET;
    TIM_OC4Init(TIM1, &TIM_OCInitStructure);
    TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable);
    TIM_ARRPreloadConfig(TIM1, ENABLE);
    /** TIM1 enable counter */
    TIM_Cmd(TIM1, ENABLE);
    /** TIM1 must has the following instruction */
    TIM_CtrlPWMOutputs(TIM1, ENABLE);
}
void TIM2_Configuration(SPIDER* spdr)
{
    /** Time base configuration */
    // Count CLK freq = 36000,Period = 7200,ie 20ms.
	TIM_TimeBaseStructure.TIM_Period = 7200;
	TIM_TimeBaseStructure.TIM_Prescaler = 200;
	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);

	/** TIM2 CH3 */
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = spdr->Right_Middle_Leg.Joint_Mid + RMLJM_OFFSET;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
	TIM_OC3Init(TIM2, &TIM_OCInitStructure);
	TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);

	/** TIM2 CH4 */
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = spdr->Right_Middle_Leg.Joint_Near + RMLJN_OFFSET;
	TIM_OC4Init(TIM2, &TIM_OCInitStructure);
	TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);
	TIM_ARRPreloadConfig(TIM2, ENABLE);

	/** TIM2 enable counter */
	TIM_Cmd(TIM2, ENABLE);
}

void TIM3_Configuration(SPIDER* spdr)
{
    /** Time base configuration */
    // Count CLK freq = 36000,Period = 7200,ie 20ms.
    TIM_TimeBaseStructure.TIM_Period = 7200;
    TIM_TimeBaseStructure.TIM_Prescaler = 200;
    TIM_TimeBaseStructure.TIM_ClockDivision = 0;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);

    /** TIM3 CH1 */
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = spdr->Right_Back_Leg.Joint_Far + RBLJF_OFFSET;
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OC1Init(TIM3, &TIM_OCInitStructure);
    TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);

    /** TIM3 CH2 */
    TIM_OCInitStructure.TIM_Pulse = spdr->Right_Back_Leg.Joint_Mid + RBLJM_OFFSET;
    TIM_OC2Init(TIM3, &TIM_OCInitStructure);
    TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);

    /** TIM3 CH3 */
    TIM_OCInitStructure.TIM_Pulse = spdr->Right_Back_Leg.Joint_Near + RBLJN_OFFSET;
    TIM_OC3Init(TIM3, &TIM_OCInitStructure);
    TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);

    /** TIM3 CH4 */
    TIM_OCInitStructure.TIM_Pulse = spdr->Left_Front_Leg.Joint_Far + LFLJF_OFFSET;
    TIM_OC4Init(TIM3, &TIM_OCInitStructure);
    TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
    TIM_ARRPreloadConfig(TIM3, ENABLE);

    /* TIM3 enable counter */
    TIM_Cmd(TIM3, ENABLE);
}

void TIM4_Configuration(SPIDER* spdr)
{
    /* Time base configuration */
    // Count CLK freq = 36000,Period = 7200,ie 20ms.
	TIM_TimeBaseStructure.TIM_Period = 7200;
	TIM_TimeBaseStructure.TIM_Prescaler = 200;
	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);

	/** TIM4 CH1 */
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = spdr->Left_Front_Leg.Joint_Mid + LFLJM_OFFSET;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
	TIM_OC1Init(TIM4, &TIM_OCInitStructure);
	TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);

	/** TIM4 CH2 */
	TIM_OCInitStructure.TIM_Pulse = spdr->Left_Front_Leg.Joint_Near - LFLJN_OFFSET;
	TIM_OC2Init(TIM4, &TIM_OCInitStructure);
	TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);

	/** TIM4 CH3 */
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = spdr->Left_Middle_Leg.Joint_Far + LMLJF_OFFSET;
	TIM_OC3Init(TIM4, &TIM_OCInitStructure);
	TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);

	/** TIM4 CH4 */
	TIM_OCInitStructure.TIM_Pulse = spdr->Left_Middle_Leg.Joint_Mid + LMLJM_OFFSET;
	TIM_OC4Init(TIM4, &TIM_OCInitStructure);
	TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);
	TIM_ARRPreloadConfig(TIM4, ENABLE);

	/* TIM 4 enable counter */
	TIM_Cmd(TIM4, ENABLE);
}
void TIM5_Configuration(SPIDER* spdr)
{
    /* Time base configuration */
	TIM_TimeBaseStructure.TIM_Period = 7200;
	TIM_TimeBaseStructure.TIM_Prescaler = 200;
	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);

	/** TIM5 CH1 */
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = spdr->Left_Middle_Leg.Joint_Near + LMLJN_OFFSET;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
	TIM_OC1Init(TIM5, &TIM_OCInitStructure);
	TIM_OC1PreloadConfig(TIM5, TIM_OCPreload_Enable);

	/** TIM5 CH2 */
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = spdr->Left_Back_Leg.Joint_Far + LBLJF_OFFSET;
	TIM_OC2Init(TIM5, &TIM_OCInitStructure);
	TIM_OC2PreloadConfig(TIM5, TIM_OCPreload_Enable);

	/** TIM5 CH3 */
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = spdr->Left_Back_Leg.Joint_Mid + LBLJM_OFFSET;
	TIM_OC3Init(TIM5, &TIM_OCInitStructure);
	TIM_OC3PreloadConfig(TIM5, TIM_OCPreload_Enable);

	/** TIM5 CH4 */
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = spdr->Left_Back_Leg.Joint_Near + LBLJN_OFFSET;
	TIM_OC4Init(TIM5, &TIM_OCInitStructure);
	TIM_OC4PreloadConfig(TIM5, TIM_OCPreload_Enable);
	TIM_ARRPreloadConfig(TIM5, ENABLE);

	/* TIM5 enable counter */
	TIM_Cmd(TIM5, ENABLE);
}

void TIM_RFLJfarRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + RFLJF_OFFSET;
    TIM_OC1Init(TIM1, &TIM_OCInitStructure);
    TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);
}
void TIM_RFLJmidRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + RFLJM_OFFSET;
    TIM_OC2Init(TIM1, &TIM_OCInitStructure);
    TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable);
}
void TIM_RFLJnearRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + RFLJN_OFFSET;
    TIM_OC3Init(TIM1, &TIM_OCInitStructure);
    TIM_OC3PreloadConfig(TIM1, TIM_OCPreload_Enable);
}
void TIM_RMLJfarRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data - RMLJF_OFFSET;
    TIM_OC4Init(TIM1, &TIM_OCInitStructure);
    TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable);
}
void TIM_RMLJmidRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + RMLJM_OFFSET;
    TIM_OC3Init(TIM2, &TIM_OCInitStructure);
    TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
}
void TIM_RMLJnearRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + RMLJN_OFFSET;
    TIM_OC4Init(TIM2, &TIM_OCInitStructure);
    TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);
}
void TIM_RBLJfarRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + RBLJF_OFFSET;
    TIM_OC1Init(TIM3, &TIM_OCInitStructure);
    TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
}
void TIM_RBLJmidRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + RBLJM_OFFSET;
    TIM_OC2Init(TIM3, &TIM_OCInitStructure);
    TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
}
void TIM_RBLJnearRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + RBLJN_OFFSET;
    TIM_OC3Init(TIM3, &TIM_OCInitStructure);
    TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);
}
void TIM_LFLJfarRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + LFLJF_OFFSET;
    TIM_OC4Init(TIM3, &TIM_OCInitStructure);
	TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
}
void TIM_LFLJmidRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + LFLJM_OFFSET;
    TIM_OC1Init(TIM4, &TIM_OCInitStructure);
	TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
}
void TIM_LFLJnearRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data - LFLJN_OFFSET;
    TIM_OC2Init(TIM4, &TIM_OCInitStructure);
	TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);
}
void TIM_LMLJfarRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + LMLJF_OFFSET;
    TIM_OC3Init(TIM4, &TIM_OCInitStructure);
	TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);
}
void TIM_LMLJmidRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + LMLJM_OFFSET;
    TIM_OC4Init(TIM4, &TIM_OCInitStructure);
	TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);
}
void TIM_LMLJnearRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + LMLJN_OFFSET;
    TIM_OC1Init(TIM5, &TIM_OCInitStructure);
	TIM_OC1PreloadConfig(TIM5, TIM_OCPreload_Enable);
}
void TIM_LBLJfarRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + LBLJF_OFFSET;
    TIM_OC2Init(TIM5, &TIM_OCInitStructure);
	TIM_OC2PreloadConfig(TIM5, TIM_OCPreload_Enable);
}
void TIM_LBLJmidRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + LBLJM_OFFSET;
    TIM_OC3Init(TIM5, &TIM_OCInitStructure);
	TIM_OC3PreloadConfig(TIM5, TIM_OCPreload_Enable);
}
void TIM_LBLJnearRenovate(u16 data)
{
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = data + LBLJN_OFFSET;
    TIM_OC4Init(TIM5, &TIM_OCInitStructure);
	TIM_OC4PreloadConfig(TIM5, TIM_OCPreload_Enable);
}

外设数据初始化stm32f10x_init.h__________点击查看代码

/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __STM32F10X_INIT_H
#define __STM32F10X_INIT_H
/* Includes ------------------------------------------------------------------*/
#include "stm32f10x_lib.h"
#include "spider.h"
#include "camera.h"

typedef struct
{
    vu32 nTime;
    vu8  start;
    vu8  flag;
}SOFTTIMER;

void SFTMRStart(u32 nTime);
u8   SFTMRCheck(void);

/* Exported functions ------------------------------------------------------- */
void Stm32f10x_Configuration(SPIDER* spdr);
void Stm32f10x_Initialization(void);
void RCC_Configuration(void);
void GPIO_Configuration(void);
void USART1_Configuration(void);
void USART3_Configuration(void);
void SysTick_Configuration(void);
void TimingDelay_Decrement(void);
void Delay(u32 nTime);

void TIM1_Configuration(SPIDER* spdr);
void TIM2_Configuration(SPIDER* spdr);
void TIM3_Configuration(SPIDER* spdr);
void TIM4_Configuration(SPIDER* spdr);
void TIM5_Configuration(SPIDER* spdr);
/** TIM8 used for the camera */
void TIM8_Configuration(CAMERA* came);
void TIM8_HorizontalRenovate(u16 data);
void TIM8_VerticalRenovate(u16 data);

/* Right front leg joint far */
void TIM_RFLJfarRenovate(u16 data);
void TIM_RFLJmidRenovate(u16 data);
void TIM_RFLJnearRenovate(u16 data);

void TIM_RMLJfarRenovate(u16 data);
void TIM_RMLJmidRenovate(u16 data);
void TIM_RMLJnearRenovate(u16 data);

void TIM_RBLJfarRenovate(u16 data);
void TIM_RBLJmidRenovate(u16 data);
void TIM_RBLJnearRenovate(u16 data);

void TIM_LFLJfarRenovate(u16 data);
void TIM_LFLJmidRenovate(u16 data);
void TIM_LFLJnearRenovate(u16 data);

void TIM_LMLJfarRenovate(u16 data);
void TIM_LMLJmidRenovate(u16 data);
void TIM_LMLJnearRenovate(u16 data);

void TIM_LBLJfarRenovate(u16 data);
void TIM_LBLJmidRenovate(u16 data);
void TIM_LBLJnearRenovate(u16 data);

#endif

5. STM32中断函数的模板文件stm32f10x_it.c和stm32f10x_it.h

STM32中断函数的模板文件stm32f10x_it.c__________点击查看代码

/* Includes ------------------------------------------------------------------*/
#include "stm32f10x_it.h"
#include "gps.h"

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
#ifndef RxBuffersize
#define RxBufferSize  5000
#endif

/* Private variables ---------------------------------------------------------*/

vu8 WiFiBuffer[100] = {0};
vu8 wificount = 0;


vu8  RxBuffer[RxBufferSize];
vu16 RxCounter = 0;
vu8  USART3_RXNE_DIS_FLAG = 0;

extern GPS MyGPS;
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/


void NMIException(void)
{
}


void HardFaultException(void)
{
  /* Go to infinite loop when Hard Fault exception occurs */
  while (1)
  {
  }
}


void MemManageException(void)
{
  /* Go to infinite loop when Memory Manage exception occurs */
  while (1)
  {
  }
}


void BusFaultException(void)
{
  /* Go to infinite loop when Bus Fault exception occurs */
  while (1)
  {
  }
}


void UsageFaultException(void)
{
  /* Go to infinite loop when Usage Fault exception occurs */
  while (1)
  {
  }
}

void DebugMonitor(void)
{
}

void SVCHandler(void)
{
}


void PendSVC(void)
{
}

void SysTickHandler(void)
{
    TimingDelay_Decrement();
}

void WWDG_IRQHandler(void)
{
}
void PVD_IRQHandler(void)
{
}
void TAMPER_IRQHandler(void)
{
}
void RTC_IRQHandler(void)
{
}
void FLASH_IRQHandler(void)
{
}
void RCC_IRQHandler(void)
{
}
void EXTI0_IRQHandler(void)
{
}
void EXTI1_IRQHandler(void)
{
}
void EXTI2_IRQHandler(void)
{
}
void EXTI3_IRQHandler(void)
{
}
void EXTI4_IRQHandler(void)
{
}
void DMA1_Channel1_IRQHandler(void)
{
}
void DMA1_Channel2_IRQHandler(void)
{
}
void DMA1_Channel3_IRQHandler(void)
{
}
void DMA1_Channel4_IRQHandler(void)
{
}
void DMA1_Channel5_IRQHandler(void)
{
}
void DMA1_Channel6_IRQHandler(void)
{
}
void DMA1_Channel7_IRQHandler(void)
{
}
void ADC1_2_IRQHandler(void)
{
}


void USB_HP_CAN_TX_IRQHandler(void)
{
}

void USB_LP_CAN_RX0_IRQHandler(void)
{
}


void CAN_RX1_IRQHandler(void)
{
}

void CAN_SCE_IRQHandler(void)
{
}


void EXTI9_5_IRQHandler(void)
{

}


void TIM1_BRK_IRQHandler(void)
{
}


void TIM1_UP_IRQHandler(void)
{
}


void TIM1_TRG_COM_IRQHandler(void)
{
}


void TIM1_CC_IRQHandler(void)
{
}

void TIM2_IRQHandler(void)
{
}

void TIM3_IRQHandler(void)
{
}

void TIM4_IRQHandler(void)
{

}

void I2C1_EV_IRQHandler(void)
{
}

void I2C1_ER_IRQHandler(void)
{
}

void I2C2_EV_IRQHandler(void)
{
}

void I2C2_ER_IRQHandler(void)
{
}

void SPI1_IRQHandler(void)
{
}

void SPI2_IRQHandler(void)
{
}

void USART1_IRQHandler(void)
{
    if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
    {
        WiFiBuffer[wificount] = USART_ReceiveData(USART1);
        wificount ++;
        if(wificount >= 100)
        {
            //USART_ITConfig(USART1, USART_IT_RXNE, DISABLE);
            wificount = 0;
        }
    }
}

void USART2_IRQHandler(void)
{
}

void USART3_IRQHandler(void)
{
    if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)
    {
        RxBuffer[RxCounter++] = USART_ReceiveData(USART3);
        if(RxCounter >= RxBufferSize)
        {
            USART_ITConfig(USART3, USART_IT_RXNE, DISABLE);
            RxCounter = 0;
            USART3_RXNE_DIS_FLAG = 1;
        }
    }
}

void EXTI15_10_IRQHandler(void)
{
}

void RTCAlarm_IRQHandler(void)
{
}

void USBWakeUp_IRQHandler(void)
{
}

void TIM8_BRK_IRQHandler(void)
{
}

void TIM8_UP_IRQHandler(void)
{
}

void TIM8_TRG_COM_IRQHandler(void)
{
}

void TIM8_CC_IRQHandler(void)
{
}

void ADC3_IRQHandler(void)
{
}

void FSMC_IRQHandler(void)
{
}

void SDIO_IRQHandler(void)
{
}

void TIM5_IRQHandler(void)
{
}

void SPI3_IRQHandler(void)
{
}

void UART4_IRQHandler(void)
{
}

void UART5_IRQHandler(void)
{
}

void TIM6_IRQHandler(void)
{
}

void TIM7_IRQHandler(void)
{
}

void DMA2_Channel1_IRQHandler(void)
{
}

void DMA2_Channel2_IRQHandler(void)
{
}

void DMA2_Channel3_IRQHandler(void)
{
}

void DMA2_Channel4_5_IRQHandler(void)
{
}

/** (C) COPYRIGHT 2008 STMicroelectronics **END OF FILE***/

STM32中断函数的模板文件stm32f10x_it.h__________点击查看代码

/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __STM32F10x_IT_H
#define __STM32F10x_IT_H

/* Includes ------------------------------------------------------------------*/
#include "stm32f10x_lib.h"
#include "stm32f10x_init.h"
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported functions ------------------------------------------------------- */

void NMIException(void);
void HardFaultException(void);
void MemManageException(void);
void BusFaultException(void);
void UsageFaultException(void);
void DebugMonitor(void);
void SVCHandler(void);
void PendSVC(void);
void SysTickHandler(void);
void WWDG_IRQHandler(void);
void PVD_IRQHandler(void);
void TAMPER_IRQHandler(void);
void RTC_IRQHandler(void);
void FLASH_IRQHandler(void);
void RCC_IRQHandler(void);
void EXTI0_IRQHandler(void);
void EXTI1_IRQHandler(void);
void EXTI2_IRQHandler(void);
void EXTI3_IRQHandler(void);
void EXTI4_IRQHandler(void);
void DMA1_Channel1_IRQHandler(void);
void DMA1_Channel2_IRQHandler(void);
void DMA1_Channel3_IRQHandler(void);
void DMA1_Channel4_IRQHandler(void);
void DMA1_Channel5_IRQHandler(void);
void DMA1_Channel6_IRQHandler(void);
void DMA1_Channel7_IRQHandler(void);
void ADC1_2_IRQHandler(void);
void USB_HP_CAN_TX_IRQHandler(void);
void USB_LP_CAN_RX0_IRQHandler(void);
void CAN_RX1_IRQHandler(void);
void CAN_SCE_IRQHandler(void);
void EXTI9_5_IRQHandler(void);
void TIM1_BRK_IRQHandler(void);
void TIM1_UP_IRQHandler(void);
void TIM1_TRG_COM_IRQHandler(void);
void TIM1_CC_IRQHandler(void);
void TIM2_IRQHandler(void);
void TIM3_IRQHandler(void);
void TIM4_IRQHandler(void);
void I2C1_EV_IRQHandler(void);
void I2C1_ER_IRQHandler(void);
void I2C2_EV_IRQHandler(void);
void I2C2_ER_IRQHandler(void);
void SPI1_IRQHandler(void);
void SPI2_IRQHandler(void);
void USART1_IRQHandler(void);
void USART2_IRQHandler(void);
void USART3_IRQHandler(void);
void EXTI15_10_IRQHandler(void);
void RTCAlarm_IRQHandler(void);
void USBWakeUp_IRQHandler(void);
void TIM8_BRK_IRQHandler(void);
void TIM8_UP_IRQHandler(void);
void TIM8_TRG_COM_IRQHandler(void);
void TIM8_CC_IRQHandler(void);
void ADC3_IRQHandler(void);
void FSMC_IRQHandler(void);
void SDIO_IRQHandler(void);
void TIM5_IRQHandler(void);
void SPI3_IRQHandler(void);
void UART4_IRQHandler(void);
void UART5_IRQHandler(void);
void TIM6_IRQHandler(void);
void TIM7_IRQHandler(void);
void DMA2_Channel1_IRQHandler(void);
void DMA2_Channel2_IRQHandler(void);
void DMA2_Channel3_IRQHandler(void);
void DMA2_Channel4_5_IRQHandler(void);

#endif /* __STM32F10x_IT_H */

6. 六足机器人外设头文件STM32F10x_conf.h

外设头文件STM32F10x_conf.h__________点击查看代码

/******************** (C) COPYRIGHT 2008 STMicroelectronics ********************
* File Name          : stm32f10x_conf.h
* Author             : MCD Application Team
* Version            : V2.0.3
* Date               : 09/22/2008
* Description        : Library configuration file.
********************************************************************************
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*******************************************************************************/

/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __STM32F10x_CONF_H
#define __STM32F10x_CONF_H

/* Includes ------------------------------------------------------------------*/
#include "stm32f10x_type.h"

/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* Uncomment the line below to compile the library in DEBUG mode, this will expanse
   the "assert_param" macro in the firmware library code (see "Exported macro"
   section below) */
/* #define DEBUG    1*/

/* Comment the line below to disable the specific peripheral inclusion */
/************************************* ADC ************************************/
#define _ADC
#define _ADC1
#define _ADC2
#define _ADC3

/************************************* BKP ************************************/
#define _BKP 

/************************************* CAN ************************************/
#define _CAN

/************************************* CRC ************************************/
#define _CRC

/************************************* DAC ************************************/
#define _DAC

/************************************* DBGMCU *********************************/
#define _DBGMCU

/************************************* DMA ************************************/
#define _DMA
#define _DMA1_Channel1
#define _DMA1_Channel2
#define _DMA1_Channel3
#define _DMA1_Channel4
#define _DMA1_Channel5
#define _DMA1_Channel6
#define _DMA1_Channel7
#define _DMA2_Channel1
#define _DMA2_Channel2
#define _DMA2_Channel3
#define _DMA2_Channel4
#define _DMA2_Channel5

/************************************* EXTI ***********************************/
#define _EXTI

/************************************* FLASH and Option Bytes *****************/
#define _FLASH
/* Uncomment the line below to enable FLASH program/erase/protections functions,
   otherwise only FLASH configuration (latency, prefetch, half cycle) functions
   are enabled */
/* #define _FLASH_PROG */

/************************************* FSMC ***********************************/
#define _FSMC

/************************************* GPIO ***********************************/
#define _GPIO
#define _GPIOA
#define _GPIOB
#define _GPIOC
#define _GPIOD
#define _GPIOE
#define _GPIOF
#define _GPIOG
#define _AFIO

/************************************* I2C ************************************/
#define _I2C
#define _I2C1
#define _I2C2

/************************************* IWDG ***********************************/
#define _IWDG

/************************************* NVIC ***********************************/
#define _NVIC

/************************************* PWR ************************************/
#define _PWR

/************************************* RCC ************************************/
#define _RCC

/************************************* RTC ************************************/
#define _RTC

/************************************* SDIO ***********************************/
#define _SDIO

/************************************* SPI ************************************/
#define _SPI
#define _SPI1
#define _SPI2
#define _SPI3

/************************************* SysTick ********************************/
#define _SysTick

/************************************* TIM ************************************/
#define _TIM
#define _TIM1
#define _TIM2
#define _TIM3
#define _TIM4
#define _TIM5
#define _TIM6
#define _TIM7
#define _TIM8

/************************************* USART **********************************/
#define _USART
#define _USART1
#define _USART2
#define _USART3
#define _UART4
#define _UART5

/************************************* WWDG ***********************************/
#define _WWDG

/* In the following line adjust the value of External High Speed oscillator (HSE)
   used in your application */
#define HSE_Value    ((u32)8000000) /* Value of the External oscillator in Hz*/

/* In the following line adjust the External High Speed oscillator (HSE) Startup 
   Timeout value */
#define HSEStartUp_TimeOut    ((u16)0x0500) /* Time out for HSE start up */

/* Exported macro ------------------------------------------------------------*/
#ifdef  DEBUG
/*******************************************************************************
* Macro Name     : assert_param
* Description    : The assert_param macro is used for function's parameters check.
*                  It is used only if the library is compiled in DEBUG mode. 
* Input          : - expr: If expr is false, it calls assert_failed function
*                    which reports the name of the source file and the source
*                    line number of the call that failed. 
*                    If expr is true, it returns no value.
* Return         : None
*******************************************************************************/ 
  #define assert_param(expr) ((expr) ? (void)0 : assert_failed((u8 *)__FILE__, __LINE__))
/* Exported functions ------------------------------------------------------- */
  void assert_failed(u8* file, u32 line);
#else
  #define assert_param(expr) ((void)0)
#endif /* DEBUG */

#endif /* __STM32F10x_CONF_H */

/******************* (C) COPYRIGHT 2008 STMicroelectronics *****END OF FILE****/

7. GPS定位程序gps.c和GPS定位程序gps.h

GPS定位程序gps.c__________点击查看代码

#include "gps.h"


void GPGGA(GPS* gps,u8 tmp);
void GPRMC(GPS* gps,u8 tmp);
void GPVTG(GPS* gps,u8 tmp);
void GPGSA(GPS* gps,u8 tmp);

void GPSInit(GPS* gps)
{
    int i = 0;
    gps->Info.ReceivingF = 0;
    gps->Info.EndF = 0;
    gps->Info.GGAF = 0;
    gps->Info.GSAF = 0;
    gps->Info.RMCF = 0;
    gps->Info.VTGF = 0;
    for(i=0; i<13; i++)
    {
        gps->GPSLongitude[i] = 0;
    }
    for(i=0; i<12; i++)
    {
        gps->GPSLatitude[i] = 0;
    }
    for(i=0; i<9; i++)
    {
        gps->GPSTime[i] = 0;
    }
}

u8 GPSJudge(GPS* gps,u8 tmp)
{
    if(tmp == '$')
    {
        gps->Info.Command = 0;
        gps->Info.ReceivingF = 1;
        gps->Info.Bytes_counter = 0;
        gps->Info.Segment = 0;
        return 0;
    }
    if(gps->Info.ReceivingF)
    {
        if(tmp == ',')
        {
            gps->Info.Segment += 1;
            gps->Info.Bytes_counter = 0;
            return 0;
        }
        if(tmp == '*')
        {
            gps->Info.ReceivingF = 0;
            gps->Info.EndF = 1;
            return 0;
        }
        if(gps->Info.Segment == 0)
        {
            if(gps->Info.Bytes_counter == 3)
            {
                switch(tmp)
                {
                case 'G':
                    gps->Info.Command = 1;
                    gps->Info.GGAF = 1;
                    break;
                case 'M':
                    gps->Info.Command = 2;
                    gps->Info.RMCF =1;
                    break;
                case 'T':
                    gps->Info.Command = 3;
                    gps->Info.VTGF = 1;
                    break;
                case 'S':
                    break;
                default:
                    gps->Info.Command = 0;
                    gps->Info.ReceivingF = 0;
                    break;
                }
            }
            if(gps->Info.Bytes_counter == 4)
            {
                if(gps->Info.Command == 0 && tmp == 'A')
                {
                    gps->Info.Command = 4;
                    gps->Info.GSAF = 1;
                }

            }
        }
        else
        {
            switch(gps->Info.Command)
            {
            case 1:
                GPGGA(gps,tmp);
                break;
            case 2:
                GPRMC(gps,tmp);
                break;
            case 3:
                GPVTG(gps,tmp);
                break;
            case 4:
                GPGSA(gps,tmp);
                break;
            default:
                break;
            }
        }
        gps->Info.Bytes_counter += 1;
    }
    gps->Info.NewByteF = 0;
    return 0;
}


void GPGGA(GPS* gps,u8 tmp)
{
    switch(gps->Info.Segment)
    {
    case 1:
        if(gps->Info.Bytes_counter == 2 || gps->Info.Bytes_counter == 5)
        {
            gps->GPSTime[gps->Info.Bytes_counter] = ':';
            gps->Info.Bytes_counter += 1;
        }
        if(gps->Info.Bytes_counter < 8)
        {
            gps->GPSTime[gps->Info.Bytes_counter] = tmp;

        }
        gps->GPSTime[8] = '\0';
        break;
    case 2:
        if(gps->Info.Bytes_counter == 3)
        {
            gps->GPSLatitude[gps->Info.Bytes_counter] = '.';
            gps->Info.Bytes_counter += 1;
            gps->GPSLatitude[11] = '\0';
        }
        if(gps->Info.Bytes_counter == 0)
        {
            gps->Info.Bytes_counter += 1;
        }
        gps->GPSLatitude[gps->Info.Bytes_counter] = tmp;
        break;
    case 3:
        gps->GPSLatitude[0] = tmp;
        break;
    case 4:
        if(gps->Info.Bytes_counter == 4)
        {
            gps->GPSLongitude[gps->Info.Bytes_counter] = '.';
            gps->Info.Bytes_counter += 1;
            gps->GPSLongitude[12] = '\0';
        }
        if(gps->Info.Bytes_counter == 0)
        {
            gps->Info.Bytes_counter += 1;
        }
        gps->GPSLongitude[gps->Info.Bytes_counter] = tmp;
        break;
    case 5:
        gps->GPSLongitude[0] = tmp;
        break;
    case 6:
        gps->GPSQuality = tmp;
        break;
    case 7:
        if(gps->Info.Bytes_counter < 2)
        {
            gps->GPSSatellite[gps->Info.Bytes_counter] = tmp;
            gps->GPSSatellite[2] = '\0';
        }
        break;
    case 9:
        if(gps->Info.Bytes_counter < 7)
        {
            gps->GPSElevation[gps->Info.Bytes_counter] = tmp;
            gps->GPSElevation[gps->Info.Bytes_counter + 1] = '\0';
        }
        break;
    default:
        break;
    }
}

void GPRMC(GPS* gps,u8 tmp)
{
    switch(gps->Info.Segment)
    {
    case 9:
        if(gps->Info.Bytes_counter < 2)
        {
            gps->GSPdate[6 + gps->Info.Bytes_counter] = tmp;
        }
        if(gps->Info.Bytes_counter > 1 && gps->Info.Bytes_counter < 4)
        {
            gps->GSPdate[1 + gps->Info.Bytes_counter] = tmp;
            gps->GSPdate[5] = '-';
        }
        if(gps->Info.Bytes_counter > 3 && gps->Info.Bytes_counter < 6)
        {
            gps->GSPdate[gps->Info.Bytes_counter - 4] = tmp;
            gps->GSPdate[2] = '-';
            gps->GSPdate[2] = '\0';
        }
        break;
    default:
        break;
    }
}
void GPVTG(GPS* gps,u8 tmp)
{
    switch(gps->Info.Segment)
    {
    case 7:
        if(gps->Info.Bytes_counter < 7)
        {
            gps->GPSSpeed[gps->Info.Bytes_counter] = tmp;
            gps->GPSSpeed[gps->Info.Bytes_counter + 1] = '$';
            gps->GPSSpeed[gps->Info.Bytes_counter + 2] = '"';
            gps->GPSSpeed[gps->Info.Bytes_counter + 3] = '/';
            gps->GPSSpeed[gps->Info.Bytes_counter + 4] = '#';
            gps->GPSSpeed[gps->Info.Bytes_counter + 5] = '\0';
        }
        break;
    default:
        break;
    }
}

void GPGSA(GPS* gps,u8 tmp)
{
    switch(gps->Info.Segment)
    {
    case 2:
        gps->GPSStatus = tmp;
        break;
    default:
        break;
    }
}

GPS定位程序gps.h__________点击查看代码

#ifndef __GPS_H
#define __GPS_H

#include "stm32f10x_lib.h"

typedef struct
{
    u8 Segment;// 逗号计数
    u8 Bytes_counter;
    u8 Command;
    u8 NewByteF;
    u8 ReceivingF;
    u8 EndF;
    u8 RMCF;
    u8 GGAF;
    u8 VTGF;
    u8 GSAF;
}GPSInfo;

typedef struct
{
    u8 GPSTime[9];      // UTC time
    u8 GPSLongitude[13];// 经度
    u8 GPSLatitude[12]; // 纬度
    u8 GPSWarn;         // 定位警告
    u8 GPSQuality;      // 定位质量
    u8 GPSStatus;       // 定位状态
    u8 GPSElevation[8]; // 海拔
    u8 GPSSatellite[3]; // 卫星
    u8 GPSSpeed[10];
    u8 GSPdate[9];      // UTC 日期
    GPSInfo Info;
}GPS;

void GPSInit(GPS* gps);
u8 GPSJudge(GPS* gps,u8 tmp);


#endif

8. 摄像机camera.c和camera.h

摄像机camera.c__________点击查看代码

#include "camera.h"
#include "stm32f10x_init.h"


void CameInit(CAMERA* came)
{
    came->Horizontal = 540;
    came->Vertical   = 540;
    came->Speed      = 30;
}

/** Camera move horizontal
** --@arg angle from 0 to 180
*/
void CameMoveHorizontal(CAMERA* came, u8 angle)
{
    u16 olddat = 0;
    u16 newdat = 0;
    newdat = 180 + angle * 4;
    olddat = came->Horizontal;
    if(newdat > olddat)
    {
        while((olddat + 10) < newdat)
        {
            olddat += 10;
            TIM8_HorizontalRenovate(olddat);
            Delay(came->Speed);
        }
    }
    else
    {
        while((olddat - 10) > newdat)
        {
            olddat -= 10;
            TIM8_HorizontalRenovate(olddat);
            Delay(came->Speed);
        }
    }
    came->Horizontal = newdat;
    Delay(came->Speed);
    TIM8_HorizontalRenovate(came->Horizontal);
}

/** Camera move vertical */
void CameMoveVertical(CAMERA* came, u8 angle)
{
    u16 olddat = 0;
    u16 newdat = 0;
    newdat = 180 + angle * 4;
    olddat = came->Vertical;
    if(newdat > olddat)
    {
        while((olddat + 10) < newdat)
        {
            olddat += 10;
            TIM8_VerticalRenovate(olddat);
            Delay(came->Speed);
        }
    }
    else
    {
        while((olddat - 10) > newdat)
        {
            olddat -= 10;
            TIM8_VerticalRenovate(olddat);
            Delay(came->Speed);
        }
    }
    came->Vertical = newdat;
    Delay(came->Speed);
    TIM8_VerticalRenovate(came->Vertical);
}

摄像机camera.h__________点击查看代码

#ifndef __CAMERA_H
#define __CAMERA_H

#include "stm32f10x_lib.h"

/** speed indicate the delay time between two step */
typedef struct
{
    u16 Horizontal;
    u16 Vertical;
    u32 Speed;
}CAMERA;

void CameInit(CAMERA* came);
/** Camera move horizontal */
void CameMoveHorizontal(CAMERA* came, u8 angle);
/** Camera move vertical */
void CameMoveVertical(CAMERA* came, u8 angle);
#endif

9. 六足机器人所有的关键数据和设置引脚

关键数据和设置引脚__________点击查看代码

最远腿长度120mm
          th1bar=joint_far（按弧度存储double）
中间腿长度85mm
          th2bar=joint_mid（按弧度存储double）
初值 th1bar = -100度
     th2bar = 135度



关节面朝前为前方
编码顺序为

左前     右前
 4	  1
 5	  2
 6	  3

黄线朝内
<1>
TIM1-CH1  PE09(最外面) sevo1 -- period = 7165
TIM1-CH2  PE11(中间)   sevo2 -- period = 7165
TIM1-CH3  PE13(最里面) sevo3 -- period = 7165

<2>
TIM1-CH4  PE14 (最外面) sevo4 -- period = 7165
TIM2-CH3  PB10 (中间)   sevo5 -- period = 7128
TIM2-CH4  PB11 (最里面) sevo6 -- period = 7128

<3>
TIM3-CH1  PC06 (最外面) sevo7 -- period = 7128
         (PA06)(PB04)-RECONFIG
TIM3-CH2  PC07 (中间)   sevo8 -- period = 7128
         (PA07)(PB05)-RECONFIG
TIM3-CH3  PC08 (最里面) Sevo9 -- period = 7128
         (PB00)-RECONFIG

<4>
TIM3-CH4  PC09(最外面) Sevo10 -- period = 7128
          (PB01)-RECONFIG
TIM4-CH1  PD12(中间)   Sevo11 -- period = 7128
TIM4-CH2  PD13(最里面) Sevo12 -- period = 7128

<5>
TIM4-CH3  PD14(最外面) Sevo13 -- period = 7128
TIM4-CH4  PD15(中间)   Sevo14 -- period = 7128
TIM5-CH1  PA00(最里面) Sevo15 -- period = 7128

<6>
TIM5-CH2  PA01(最外面) Sevo16 -- period = 7128
TIM5-CH3  PA02(中间)   Sevo17 -- period = 7128
TIM5-CH4  PA03(最里面) Sevo18 -- period = 7128


calculation:


弧度 -- 角度 --- 占空比 ---  period --- pulse
          0        2.5%       7200       180
          45       5.0%       7200       360
          90       7.5%       7200       540
          135      10.0%      7200       720
          180      12.5%      7200       900


=================camera===========================
TIM8 CH1 (PC06)
--Horizontal

TIM8 CH2 (PC07)
--Vertical
=================camera===========================


=PB0
=PB1
=PE9
=PE11
=PE13
=PE14
=PB10
=PB11
=PD12
=PD13
=PD14
=PD15
=PC6---CAMERA==
=PC7---CAMERA==
PC8
PC9
PA8


=PA9---usart1===TX
=PA10--usart1===RX
PA11
PA15
PB3
=PB4
=PB5
PB6
PB7
PB8
PB9
=PA0
=PA1
=PA2
=PA3
PA7
PA6

PD08---USART3_TX
PD09---USART3_RX