四足机器人2

// 智林Z2101/03开发板试验程序
// GPIO输出试验
// 刘笑然
// 智林测控技术研究所
//
// 2006.12.1

// 引入相关芯片的头文件
#include
extern unsigned char UART0_GetCh(unsigned char *Ch);
// 发光管接在P0.3引脚
#define pin17 (1<<17) //从上到下分别是六个舵机的控制引脚
#define pin19 (1<<19)
#define pin21 (1<<21)
#define pin23 (1<<23)
#define pin25 (1<<25)
#define pin27 (1<<27)


void DelayNS (int dly) {
int i;

for ( ; dly>0; dly--)
for (i=0; i<700; i++);
}

void Duoji1(int n,int m)
{
for(i=0;i<30;i++)
{
IOSET=pin17;
DelayNS(m);
IOCLR=pin17;
DelayNS(n-m);
}
}

void Duoji2(int n,int m)
{
for(i=0;i<30;i++)
{
IOSET=pin19;
DelayNS(m);
IOCLR=pin19;
DelayNS(n-m);
}
}
void Duoji3(int n,int m)
{
for(i=0;i<30;i++)
{
IOSET=pin21;
DelayNS(m);
IOCLR=pin21;
DelayNS(n-m);
}
}

void Duoji4(int n,int m)
{
for(i=0;i<30;i++)
{
IOSET=pin23;
DelayNS(m);
IOCLR=pin23;
DelayNS(n-m);
}
}

void Duoji5(int n,int m)
{
for(i=0;i<30;i++)
{
IOSET=pin25;
DelayNS(m);
IOCLR=pin25;
DelayNS(n-m);
}
}

void Duoji6(int n,int m)
{
for(i=0;i<30;i++)
{
IOSET=pin27;
DelayNS(m);
IOCLR=pin27;
DelayNS(n-m);
}
}
int main(void) {
int i, n=200;
unsigned char m=8;
IODIR=LED; //设置LED控制口为输出
//UART0_init();
while(1){
// if(UART0_GetCh(&m))
// ;
for(i=0;i<30;i++)
{
IOSET=LED;
DelayNS(m);
IOCLR=LED;
DelayNS(n-m);
}
m++;
if(m>17)
m=0;
}
init();



}



//初始函数，及程序加载后，机械臂将处于收缩状态，这里需要调用各个舵机的控制函数
void init ()
{

Duoji1(200, m) ; //参数m的给出需要经过调试 ，给出理想参数
Duoji2(200, m) ;
Duoji3(200, m) ;
Duoji4(200, m) ;
Duoji5(200, m) ;
Duoji6(200, m) ;

}

void gohead()
{
//前行
int i;
for(i=0;i<6;i++)
{
Duoji1(200, 8) ;
Duoji2(200, 10) ;
Duoji1(200, 8) ;

Duoji3(200, 8) ;
Duoji4(200, 10) ;
Duoji3(200, 8) ;

Duoji5(200, 8) ;
Duoji6(200, 10) ;
Duoji5(200, 8) ;

Duoji2(200, 10) ;
Duoji4(200, 10) ;
Duoji6(200, 10) ;
}

}

void turnleft()
{
//左转
int i;
for(i=0;i<6;i++)
{
Duoji1(200, 8) ;
Duoji2(200, 10) ;
Duoji1(200, 8) ;

Duoji3(200, 8) ;
Duoji4(200, 10) ;
Duoji3(200, 8) ;

Duoji5(200, 8) ;
Duoji6(200, 10) ;
Duoji5(200, 8) ;

Duoji2(200, 10) ;
Duoji4(200, 10) ;
Duoji6(200, 10) ;
}
}



//取书动作，
void move()
{
IODIR=0x3e; //0011，1110 是3E，用p0.1-p0.5 做舵机控制输出，
Duoji1(200, m) ; //m的值是不同的。
Duo

ji2(200, m) ;
Duoji3(200, m) ;
Duoji4(200, m) ;
Duoji5(200, m) ; //取书动作需要一个爪子张开的动作
DelayNS(20); //延时等待一会
Duoji5(200, m) ; //取完书需要将书加紧，所以对爪子要进行两次操作
}

//放书动作
void moverelase()
{
IODIR=0x3e; //0011，1110 是3E，用p0.1-p0.5 做舵机控制输出，

Duoji2(200, m) ;
Duoji3(200, m) ; //这里几个关节需要协调配合，及顺序和幅度，需要多次调用同一个舵机。
Duoji1(200, m) ;
Duoji4(200, m) ;
Duoji5(200, m) ; //取书动作需要一个爪子张开的动作 ,但是动作幅度很小
//DelayNS(20); //延时等待一会
Duoji5(200, m) ; //取完书需要将书加紧，所以对爪子要进行两次操作

Duoji3(200, m) ; //放完书后，机械臂需要收回，而这个过程，要配合完成。
Duoji1(200, m) ;
Duoji4(200, m) ;
}