mega16狭窄足机器人程序 -

#include "mega16.h"
#include "delay.h"
#include<math.h>
float Now_s1=90,Now_s2=90,Now_s3=90,Now_s4=90,Now_s5=90,Now_s6=90,Now_F=0;
#define uchar unsigned char
#define uint unsigned int
void Init(void)
{
DDRA=0x00;
PORTA=0x00;
DDRB=0xff;
PORTB=0xff;
DDRC=0xff;
PORTC=0xff;
DDRD=0xff;
PORTD=0xff;
}
void Servo_Control(uint Angel,uchar Channel)
{
if(Channel>=1&&Channel<=6)
{
if(Angel<0)Angel=0;
if(Angel>180)Angel=180;
switch(Channel)
{
case 1: Angel=Angel*5-10; break; //40 200
case 2: Angel=Angel*5+8; break; //45 200
case 3: Angel=Angel*5+10; break; //40 203
case 4: Angel=Angel*5-2; break; //40 205
case 5: Angel=Angel*5+3; break; //40 205 //+6
case 6: Angel=Angel*4.85+30; break; //40 200
}
Angel+=10;
if(Angel<0||Angel>980)Angel=0;
else if(Angel>950)Angel=950;
PORTC=0x00;
PORTB=0x00;
PORTD.2=0;
delay_us(50);
Channel=7-Channel;
Channel=Channel-1;
PORTC=Channel*4+Angel/256;
PORTB=(uchar)Angel;
delay_us(50);
PORTD.2=1;
delay_us(100);
PORTC=0x00;
PORTB=0x00;
PORTD.2=0;
}
}
void ServoReset(void)
{
uchar i=0;
for(i=1;i<=6;i++)
{
Servo_Control(90,i);
}
}
#define TIME_PER_STEP 5
//type:0正常 1加速 2减速 3加速减速 4脚腕加速其他减速 5脚腕减速其他加速
void Do(uchar s1,uchar s2,uchar s3,uchar s4,uchar s5,uchar s6,uint steps,int forward,uchar type)
{
float Step_s1=0,Step_s2=0,Step_s3=0,Step_s4=0,Step_s5=0,Step_s6=0,Step_F=0;
uint i=0;
Step_s1=(s1-Now_s1)/steps;
Step_s2=(s2-Now_s2)/steps;
Step_s3=(s3-Now_s3)/steps;
Step_s4=(s4-Now_s4)/steps;
Step_s5=(s5-Now_s5)/steps;
Step_s6=(s6-Now_s6)/steps;
Step_F=(forward-Now_F)/steps;
if(type==0)
{
for(i=0;i<steps;i++)
{
Now_F+=Step_F;
Now_s1+=Step_s1;
Now_s2+=Step_s2;
Now_s3+=Step_s3;
Now_s4+=Step_s4;
Now_s5+=Step_s5;
Now_s6+=Step_s6;
Servo_Control((uchar)(Now_s1-Now_F),1);
Servo_Control((uchar)Now_s2,2);
Servo_Control((uchar)Now_s3,3);
Servo_Control((uchar)(Now_s4+Now_F),4);
Servo_Control((uchar)Now_s5,5);
Servo_Control((uchar)Now_s6,6);
delay_ms(TIME_PER_STEP);
}
}
else
if(type==1)
{
for(i=0;i<steps;i++)
{
Step_F=((float)i/(float)steps)*((float)i/(float)steps)*(forward-Now_F)+Now_F;
Step_s1=((float)i/(float)steps)*((float)i/(float)steps)*(s1-Now_s1)+Now_s1;
Step_s2=((float)i/(float)steps)*((float)i/(float)steps)*(s2-Now_s2)+Now_s2;
Step_s3=((float)i/(float)steps)*((float)i/(float)steps)*(s3-Now_s3)+Now_s3;
Step_s4=((float)i/(float)steps)*((float)i/(float)steps)*(s4-Now_s4)+Now_s4;
Step_s5=((float)i/(float)steps)*((float)i/(float)steps)*(s5-Now_s5)+Now_s5;
Step_s6=((float)i/(float)steps)*((float)i/(float)steps)*(s6-Now_s6)+Now_s6;
Servo_Control((uchar)Step_s1-Step_F,1);
Servo_Control((uchar)Step_s2,2);
Servo_Control((uchar)Step_s3,3);
Servo_Control((uchar)Step_s4+Step_F,4);
Servo_Control((uchar)Step_s5,5);
Servo_Control((uchar)Step_s6,6);
delay_ms(TIME_PER_STEP);
}
}
else
if(type==2)
{
for(i=0;i<steps;i++)
{
Step_F=(1.0-(1.0*((float)i/(float)steps-1.0)*((float)i/(float)steps-1.0)))*(forward-Now_F)+Now_F;
Step_s1=(1.0-(1.0*((float)i/(float)steps-1.0)*((float)i/(float)steps-1.0)))*(s1-Now_s1)+Now_s1;
Step_s2=(1.0-(1.0*((float)i/(float)steps-1.0)*((float)i/(float)steps-1.0)))*(s2-Now_s2)+Now_s2;
……………………
…………限于本文篇幅余下代码请从51黑下载附件…………

复制代码