mega16狹窄足機器人程序

ID:611223 · 發(fā)表于 2020-10-19 14:43

基于的單片機機器人狹窄足機器人實例

#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黑下載附件…………

復制代碼

ID:1 · 發(fā)表于 2020-10-19 20:53

本帖需要重新編輯補全電路原理圖,源碼,詳細說明與圖片即可獲得100+黑幣(帖子下方有編輯按鈕)

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

mega16狹窄足機器人程序