Robofun 機器人論壇

標題: 新手求助 [打印本頁]

作者: sam21408 時間: 2016-12-5 22:54
標題: 新手求助
本帖最後由 sam21408 於 2016-12-5 23:00 編輯

我看著書的解說，設計一個超音波避障車，結果車子只會前進幾秒，後退幾秒，右後轉，一直重複，請問我哪裡打錯，我是新手海請各位高手幫幫忙。
#include <Servo.h>
Servo servo;
const int ECHOPIN=2;
const int TRIGPIN=4;
const int negR=7;
const int posR=8;
const int negL=12;
const int posL=13;
const int pwmR=5;
const int pwmL=6;
const int Rspeed=150;
const int Lspeed=150;
const int rotSpeed=180;
unsigned long Rdistance;
unsigned long Ldistance;
unsigned long Cdistance;
void setup()
{
  pinMode(posR,OUTPUT);
  pinMode(negR,OUTPUT);
  pinMode(posL,OUTPUT);
  pinMode(negL,OUTPUT);
  pinMode(TRIGPIN,OUTPUT);
  pinMode(ECHOPIN, INPUT);

  servo.attach(3);
  servo.write(90);
}

void loop()
{
  servo.write(90);
  delay(500);
  Cdistance=ping(TRIGPIN );
  if(Cdistance<8)
  {
pause(0,0);

servo.write(45);
delay(500);
Rdistance=ping(TRIGPIN);
servo.write(135);
delay(500);
Ldistance=ping(TRIGPIN);
servo.write(90);
if(Rdistance<8 && Ldistance<8)
{
   back(Rspeed,Lspeed);

   delay(2000);
   right(rotSpeed,rotSpeed);

   delay(500);
   forward(Rspeed,Lspeed);
}
else if(Rdistance>Ldistance)
{
   right(rotSpeed,rotSpeed);

   delay(500);
}
else if(Ldistance>Rdistance)
{
   left(rotSpeed,rotSpeed);

   delay(500);
}
  }
  else

forward(Rspeed,Lspeed);
}
int ping(int)
{
unsigned long cm,duration;

digitalWrite(TRIGPIN,HIGH);
delayMicroseconds(2);
digitalWrite(TRIGPIN,HIGH);
delayMicroseconds(10);
digitalWrite(TRIGPIN,LOW);
duration=pulseIn(ECHOPIN,HIGH);
cm=duration/29/2;
return cm;

}
void forward(byte RmotorSpeed, byte LmotorSpeed)
{
analogWrite(pwmR,RmotorSpeed);
analogWrite(pwmL,LmotorSpeed);
digitalWrite(posR,HIGH);
digitalWrite(negR,LOW);
digitalWrite(posL,LOW);
digitalWrite(negL,HIGH);
}
void back(byte RmotorSpeed, byte LmotorSpeed)
{
analogWrite(pwmR,RmotorSpeed);
analogWrite(pwmL,LmotorSpeed);
digitalWrite(posR,LOW);
digitalWrite(negR,HIGH);
digitalWrite(posL,HIGH);
digitalWrite(negL,LOW);
}
void pause(byte RmotorSpeed, byte LmotorSpeed)
{
analogWrite(pwmR,RmotorSpeed);
analogWrite(pwmL,LmotorSpeed);
digitalWrite(posR,LOW);
digitalWrite(negR,LOW);
digitalWrite(posL,LOW);
digitalWrite(negL,LOW);
}
void right(byte RmotorSpeed, byte LmotorSpeed)
{
analogWrite(pwmR,RmotorSpeed);
analogWrite(pwmL,LmotorSpeed);
digitalWrite(posR,LOW);
digitalWrite(negR,LOW);
digitalWrite(posL,LOW);
digitalWrite(negL,HIGH);
}
void left(byte RmotorSpeed, byte LmotorSpeed)
{
analogWrite(pwmR,RmotorSpeed);
analogWrite(pwmL,LmotorSpeed);
digitalWrite(posR,HIGH);
digitalWrite(negR,LOW);
digitalWrite(posL,LOW);
digitalWrite(negL,LOW);
}

歡迎光臨 Robofun 機器人論壇 (https://robofun.net/forum/)