91视频在线免费观看,精品99久久,日韩一区二区三区在线播放

//=======================================================
// 975=轉向輪向左轉車身左側電機1
//=======================================================
#include <Servo.h>
#define SER1_BAUD 115200
/*電機驅動引腳*/
#define PWMB_IN1 11 //定義IN1引腳
#define PWMB_IN2 6 //定義IN2引腳
#define PWMA_IN1 5 //定義IN3引腳
#define PWMA_IN2 3 //定義IN4引腳
#define Servor_Pin 7
#define BEEP_PIN 12 //定義蜂鳴器引腳D6
#define NLED_PIN 13 //定義呼吸燈引腳D11
#define IR1 A1
#define IR2 A0
#define IR3 A3
#define IR4 A2
#define IR5 10
#define IH2 0
#define IH3 A5
#define IH4 A4
#define NLED_MS_BETWEEN 500
#define DUOJI_MS_BETWEEN 20.000
int Out1,Out2,Out3,Out4,Out5,Out2H,Out3H,Out4H;
int pwm_value=1435;
int pwm_R=1140;
int pwm_L=1860;
int Echo =12;
int Trig =13;
int Distance = 50;
double sv=130; //直線速度
double bv=130; //24轉速
double cv=140; //curve彎道速度
int c1=0; //矯正指示值
int c5=0;
int fore=0; //前部判斷
int f=0; //前部觸發(fā)
int dj=0; //舵機
int hou=0; //尾部判斷
int h=0; //尾部光電
int z=0; //障礙位置設定L==0,R==1
double v=1; //累計車速
char val=0;
double num1=1;
double num2=1;
double num3=1;
double num4=1;
double num5=1;
double n1=1;
double n2=1;
double n4=1;
double n5=1;
double st=1; //stop定時
unsigned long time1;
unsigned long timel=0;
unsigned long timer=0;
unsigned long dif=0;
Servo myservo; //創(chuàng)建舵機伺服對象數(shù)組
unsigned char handle_ms_between( unsigned long *time_ms, unsigned int ms_between);
//void handle_nled();
void Motor_Forward(char motor,char pwm);
void Motor_Back(char motor,char pwm); //電機反轉
void Motor_Right();
void Motor_Left();
void Distance_test();
void dida(u8 times, u8 frequency);
void rear_judge();
void fore_judge();
void correct();
void R_count();
void L_count();
void dis();
void detect();
void count();
void setup()
{
pinMode(BEEP_PIN, OUTPUT);
// pinMode(NLED_PIN, OUTPUT);
pinMode(IR1, INPUT);
pinMode(IR2, INPUT);
pinMode(IR3, INPUT);
pinMode(IR4, INPUT);
pinMode(IR5, INPUT);
pinMode(IH2, INPUT);
pinMode(IH3, INPUT);
pinMode(IH4, INPUT);
pinMode(Echo, INPUT);
pinMode(Trig, OUTPUT);
Serial.begin(SER1_BAUD);
myservo.attach(Servor_Pin);
myservo.writeMicroseconds(pwm_value);
dida(1, 1000);
delay(1000);
}
void loop()
{
while(1)
{
Distance_test();
time1=millis();
Serial.print("time:");
detect();
fore_judge(); //前輪位置判斷
rear_judge(); //后輪位置判斷
count(); //計時
// dif=abs(timel-timer);
// Serial.print("timer:");
// Serial.println(timer);
// Serial.print("timel:");
// Serial.println(timel);
//if(dif<=1600 && dif>=10)
// {
// correct();
// Motor_Forward(1,255);
// Motor_Forward(2,255);
// delay(5000);
// Serial.print("dif:");
// Serial.println(dif);
// }
// else
// {;}
// Serial.println(c1);
//=======================================================
// 主程序
//=======================================================
if(Out1==1 && Out2==1 && Out3==0 && Out4==1 && Out5==1) //直行
{
v=num3;
if(hou==3)
{
Motor_Forward(1,sv-v);
Motor_Forward(2,sv-v);
myservo.writeMicroseconds(pwm_value);
}
else
{
v=num3;
v=v*0.9;
Motor_Forward(1,sv-v);
Motor_Forward(2,sv-v);
}
}
else if(Out1==0 && Out2==1 && Out3==1 && Out4==1 && Out5==1) //1左轉
{
while( Out4==1 )
{
Motor_Left();
timel=millis();
}
num3=0;
}
else if(Out1==1 && Out2==1 && Out3==1 && Out4==0 && Out5==1) //4右轉
{
v=num3;
v=v*0.7;
dj=1;
if(hou==3)
{
Motor_Forward(1,bv-v); //bv=160,v=110,num=20
Motor_Forward(2,bv-20-v-num4);
myservo.writeMicroseconds(pwm_value-80);
}
else if(hou==4 && h==0)
{
Motor_Forward(1,bv-v);
Motor_Forward(2,bv-20-v-num4);
myservo.writeMicroseconds(pwm_value-40);
}
else if(hou==2 && h==0)
{
Motor_Forward(1,bv-v);
Motor_Forward(2,bv-20-v-num4);
myservo.writeMicroseconds(pwm_value-130);
}
else if(h==1 && hou==4)
{
Motor_Forward(1,bv-v);
Motor_Forward(2,bv-20-v-num4);
myservo.writeMicroseconds(pwm_value);
}
else if(h==1 && hou==2)//-
{
Motor_Forward(1,bv-20-v-num4);
Motor_Forward(2,bv-v);
myservo.writeMicroseconds(pwm_value-180);
}
else
{
v=num3;
Motor_Forward(1,bv-v);
Motor_Forward(2,bv-v);
}
}
else if(Out1==1 && Out2==1 && Out3==1 && Out4==1 && Out5==0) //5右轉
{
while( Out2==1)
{
Motor_Right();
timer=millis();
}
num3=0;
}
else if(Out1==1 && Out2==0 && Out3==1 && Out4==1 && Out5==1) //2左轉
{
v=num3;
v=v*0.7;
dj=1;
if(hou==3) //bv=160,v=110,num=20
{
Motor_Forward(1,bv-20-v-num2);
Motor_Forward(2,bv-v);
myservo.writeMicroseconds(pwm_value+80);
}
else if(hou==2 && h==0)
{
Motor_Forward(1,bv-20-v-num2);
Motor_Forward(2,bv-v);
myservo.writeMicroseconds(pwm_value+40);
}
else if(hou==4 && h==0)
{
Motor_Forward(1,bv-v);
Motor_Forward(2,bv-30-v);
myservo.writeMicroseconds(pwm_value+130);
}
else if(h==1 && hou==2)
{
Motor_Forward(1,bv-20-v-num2);
Motor_Forward(2,bv-v);
myservo.writeMicroseconds(pwm_value);
}
else if(h==1 && hou==4)
{
Motor_Forward(1,bv-v);
Motor_Forward(2,bv-20-v-num2);
myservo.writeMicroseconds(pwm_value+180);
}
else
{
Motor_Forward(1,bv-v);
Motor_Forward(2,bv-v);
}
}
else if(Out1==0 && Out2==0 && Out3==0 && Out4==0 && Out5==0) //stop計數(shù)
{
if(st>=100)
{
Motor_Forward(1,255);
Motor_Forward(2,255);
break;//還是用return
}
else
{;}
}
else
{
v=num3;
Motor_Forward(1,sv-v);
Motor_Forward(2,sv-v);
}
dis();
}
}
void Motor_Right() //5右轉調用程序 wandaozhuanjiaodizeng
{
detect();
fore_judge();
rear_judge();
R_count();
if(Out1==1 && Out2==1 && Out3==1 && Out4==1 && Out5==0)
{
if(dj==1 && h==0)
{
Motor_Forward(1,170);
Motor_Forward(2,80);
myservo.writeMicroseconds(pwm_value-360);
}
else if(dj==1 && h==1 && hou==3)
{
Motor_Forward(1,170);
Motor_Forward(2,80);
myservo.writeMicroseconds(pwm_value-360);
}
else if(dj==1 && h==1 && hou==2) //-
{
Motor_Forward(1,170);
Motor_Forward(2,80);
myservo.writeMicroseconds(pwm_value-360);
}
else if(dj==1 && h==1 && hou==4)
{
Motor_Forward(1,170);
Motor_Forward(2,80);
myservo.writeMicroseconds(pwm_value-360);
}
else //*******
{
Motor_Forward(1,160);
Motor_Forward(2,80);
myservo.writeMicroseconds(pwm_value-360);
}
//Serial.print("55");
}
else if(Out1==1 && Out2==1 && Out3==1 && Out4==0 && Out5==1)
{
dj=0;
Motor_Forward(1,cv-n4); //cv=150
Motor_Forward(2,cv-60-n4);
myservo.writeMicroseconds(pwm_value-310);
// Serial.print("54");
}
else if(Out1==1 && Out2==1 && Out3==0 && Out4==1 && Out5==1)
{
Motor_Forward(1,cv);
Motor_Forward(2,cv-30);
myservo.writeMicroseconds(pwm_value-130);
// Serial.print("53");
}
else if(Out1==1 && Out2==1 && Out3==1 && Out4==1 && Out5==1 && f==1 && fore==0)
{
dj=0;
if(h==0 )
{
Motor_Forward(1,170);
Motor_Forward(2,100);
myservo.writeMicroseconds(pwm_value-450);
// Serial.print("zou");
}
else if(h==1 && hou==3)
{
Motor_Forward(1,170);
Motor_Forward(2,100);
myservo.writeMicroseconds(pwm_value-450);
// Serial.print("zou");
}
else if(h==1 && hou!=3) //-
{
Motor_Forward(1,170);
Motor_Forward(2,100);
myservo.writeMicroseconds(pwm_value-450);
// Serial.print("zou");
}
else
{
Motor_Forward(1,cv);
Motor_Forward(2,cv);
}
}
else
{
Motor_Forward(1,cv);
Motor_Forward(2,cv);
}
}
void Motor_Left() //1左轉調用程序
{
detect();
fore_judge();
rear_judge();
L_count();
if(Out1==0 && Out2==1 && Out3==1 && Out4==1 && Out5==1)
{
if(dj==1 && h==0)
{
myservo.writeMicroseconds(pwm_value+360);
Motor_Forward(1,80);
Motor_Forward(2,170);
}
else if(dj==1 && h==1 && hou==3)
{
myservo.writeMicroseconds(pwm_value+360);
Motor_Forward(1,80);
Motor_Forward(2,170);
}
else if(dj==1 && h==1 && hou==2)
{
Motor_Forward(1,80);
Motor_Forward(2,170);
myservo.writeMicroseconds(pwm_value+360);
}
else if(dj==1 && h==1 && hou==4)
{
Motor_Forward(1,80);
Motor_Forward(2,170);
myservo.writeMicroseconds(pwm_value+360);
}
else
{
Motor_Forward(1,120);
Motor_Forward(2,160);
myservo.writeMicroseconds(pwm_value+360);
}
}
else if(Out1==1 && Out2==0 && Out3==1 && Out4==1 && Out5==1)
{
dj=0;
Motor_Forward(1,cv-60-n2);//-
Motor_Forward(2,cv-n2);
myservo.writeMicroseconds(pwm_value+310);
// Serial.print("12");
}
else if(Out1==1 && Out2==1 && Out3==0 && Out4==1 && Out5==1)
{
Motor_Forward(1,cv-30);//-
Motor_Forward(2,cv);
myservo.writeMicroseconds(pwm_value+130);
// Serial.print("13");
}
else if(Out1==1 && Out2==1 && Out3==1 && Out4==1 && Out5==1 && f==1 && fore==0)//-
{
dj=0;
if(h==0)
{
Motor_Forward(1,100);
Motor_Forward(2,170);
myservo.writeMicroseconds(pwm_value+450);
// Serial.print("you");
}
else if(h==1 && hou==3)
{
Motor_Forward(1,100);
Motor_Forward(2,170);
myservo.writeMicroseconds(pwm_value+450);
// Serial.print("you");
}
else if(h==1 && hou!=3)//-
{
Motor_Forward(1,100);
Motor_Forward(2,170);
myservo.writeMicroseconds(pwm_value+450);
}
else
{
Motor_Forward(1,160);
Motor_Forward(2,160);
}
}
else
{
Motor_Forward(1,cv);
Motor_Forward(2,cv);
}
}
void correct()
{
detect();
if(c1==1)
{
while(Out3==0)
{
Out3 = digitalRead(IR3);
myservo.writeMicroseconds(pwm_value+100);
Motor_Forward(1,170);
Motor_Forward(2,230);
}
}
else if(c5==1)
{
while(Out3==0)
{
Out3 = digitalRead(IR3);
myservo.writeMicroseconds(pwm_value-100);
Motor_Forward(1,230);
Motor_Forward(2,170);
}
}
else
{;}
}
void rear_judge()
{
if(Out2H==0 && Out3H==1 && Out4H==1)
{ hou=2;h=0; }
else if(Out2H==1 && Out3H==0 && Out4H==1)
{ hou=3; h=0; }
else if(Out2H==1 && Out3H==1 && Out4H==0)
{ hou=4; h=0; }
else
{h=1;}
}
void fore_judge()
{
if (Out2==0 || Out3==0 || Out4==0)
{
fore=1;
f=0;
}
else if(Out1==0 && Out2==1 && Out3==1 && Out4==1 && Out5==1)
{
fore=0;f=0;
c1=1;
c5=0;
}
else if(Out1==1 && Out2==1 && Out3==1 && Out4==1 && Out5==0)
{
fore=0;f=0;
c5=1;
c1=0;
}
else
{
f=1;
}
}
void dis()
{
if(Distance < 35) //避障
{
while(Distance<35)//再次判斷是否有障礙物，若有則轉動方向后，繼續(xù)判斷
{
Distance_test();//測量前方距離
if(z==1)
{
Motor_Forward(1,200);
Motor_Forward(2,150);
myservo.writeMicroseconds(1160);// 右轉 400ms
}
else
{
Motor_Forward(1,200);
Motor_Forward(2,150);
myservo.writeMicroseconds(1700);// 右轉 400ms
}
}
if(z==0)
{
Motor_Forward(1,180);
Motor_Forward(2,180);
myservo.writeMicroseconds(1430);// 右轉 400ms
delay(1000);
}
else
{
Motor_Forward(1,180);
Motor_Forward(2,180);
myservo.writeMicroseconds(1430);// 右轉 400ms
delay(1000);
}
if(z==0)
{
Out5 = digitalRead(IR5);
while(Out5==0)
{
Out5 = digitalRead(IR5);
Motor_Forward(1,200);
Motor_Forward(2,150);
myservo.writeMicroseconds(1160);// 右轉 400ms
}
}
else
{
Out1 = digitalRead(IR1);
while(Out1==0)
{
Out1 = digitalRead(IR1);
Motor_Forward(1,200);
Motor_Forward(2,150);
myservo.writeMicroseconds(1700);// 右轉 400ms
}
}
}
}
void R_count()
{
if(Out1==1 && Out2==1 && Out3==1 && Out4==0 && Out5==1) //4右轉計數(shù)
{
n4=n4+0.1;
if(n4>=50)
{ n4=50; }
//Serial.print("num4:"); //輸出num3
//Serial.println(num4); //顯示次數(shù)
}
else { n4=1; }
if(Out1==1 && Out2==1 && Out3==1 && Out4==1 && Out5==0) //4右轉計數(shù)
{
n5=n5+0.1;
if(n5>=50)
{ n5=50; }
//Serial.print("num4:"); //輸出num3
//Serial.println(num4); //顯示次數(shù)
}
else { n5=1; }
}
void L_count()
{
if(Out1==1 && Out2==0 && Out3==1 && Out4==1 && Out5==1) //4右轉計數(shù)
{
n2=n2+0.1;
if(n2>=30)
{ n2=30; }
//Serial.print("num4:"); //輸出num3
//Serial.println(num4); //顯示次數(shù)
}
else { n2=1; }
if(Out1==0 && Out2==1 && Out3==1 && Out4==1 && Out5==1) //4右轉計數(shù)
{
n1=n1+0.1;
if(n1>=30)
{ n1=30; }
//Serial.print("num4:"); //輸出num3
//Serial.println(num4); //顯示次數(shù)
}
else { n1=1; }
}
void Distance_test() // 量出前方距離
{
digitalWrite(Trig, LOW); // 給觸發(fā)腳低電平2μs
delayMicroseconds(2);
digitalWrite(Trig, HIGH); // 給觸發(fā)腳高電平10μs，這里至少是10μs
delayMicroseconds(10);
digitalWrite(Trig, LOW); // 持續(xù)給觸發(fā)腳低電
float Fdistance = pulseIn(Echo, HIGH); // 讀取高電平時間(單位：微秒)
Fdistance= Fdistance/58; //為什么除以58等于厘米， Y米=（X秒*344）/2
Distance = Fdistance;
if(Distance>=60)
{
Distance=60;
}
else if(Distance<=8)
{
Distance=60;
}
// Serial.println(Distance); //顯示距離
}
void handle_nled()
{
static unsigned long systick_ms_bak = 0;
if(!handle_ms_between(&systick_ms_bak, NLED_MS_BETWEEN))return;
digitalWrite(NLED_PIN, val);
val = ~val;
}
void dida(u8 times, u8 frequency)
{
for(byte i = 0; i < times; i++ )
{
digitalWrite(BEEP_PIN, LOW);
delay(frequency);
delay(frequency);
digitalWrite(BEEP_PIN, HIGH );
delay(frequency);
delay(frequency);
}
}
unsigned char handle_ms_between( unsigned long *time_ms, unsigned int ms_between)
{
if(millis() - *time_ms < ms_between)
{
return 0;
}
else
{
*time_ms = millis();
return 1;
}
}
void detect()
{
Out1 = digitalRead(IR1);
Out2 = digitalRead(IR2);
Out3 = digitalRead(IR3);
Out4 = digitalRead(IR4);
Out5 = digitalRead(IR5);
Out2H = digitalRead(IH2);
Out3H = digitalRead(IH3);
Out4H = digitalRead(IH4);
}
void Motor_Forward(char motor,char pwm) //電機正轉
{
if(motor==1)
{
analogWrite(PWMA_IN1,pwm);
analogWrite(PWMA_IN2,255);
}
else if(motor==2)
{
analogWrite(PWMB_IN1,pwm);
analogWrite(PWMB_IN2,255);
}
}
void Motor_Back(char motor,char pwm) //電機反轉
{
if(motor==1)
{
analogWrite(PWMA_IN1,255);
analogWrite(PWMA_IN2,pwm);
}
else if(motor==2)
{
analogWrite(PWMB_IN1,255);
analogWrite(PWMB_IN2,pwm);
}
}
void count()
{
if(Out1==1 && Out2==1 && Out3==0 && Out4==1 && Out5==1) //3直行計數(shù)
{
v=v+0.5;
if(v>=sv-10)
{ v=sv-10; }
num3=v;
}
else {;}
if(Out1==1 && Out2==1 && Out3==1 && Out4==0 && Out5==1) //4右轉計數(shù)
{
num4=num4+0.2;
if(num4>=20)
{ num4=20; }
}
else { num4=1; }
if(Out1==1 && Out2==0 && Out3==1 && Out4==1 && Out5==1) //2左轉計數(shù)
{
num2=num2+0.2;
if(num2>=20)
{ num2=20; }
}
else { num2=1; }
if(Out1==0 && Out2==0 && Out3==0 && Out4==0 && Out5==0) //stop計數(shù)
{
st=st+0.2;
}
else { st=1; }
}

復制代碼

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