arduino 5路紅外循跡。純新手 不會發代碼到論壇
#define STOP 0
#define FORWARD 1
#define BACKWARD 2
#define STURNLEFT 3
#define STURNRIGHT 4
#define TURNLEFT 5
#define TURNRIGHT 6
#define TURNRIGHT90 7
#define TURNLEFT90 8
#define TURNLEFT360 9
#define TURNRIGHT360 10
#include <Wire.h>
#include <VL53L0X.h>
VL53L0X sensor;
int leftA_PIN = A0;
int leftB_PIN = A1;
int righA_PIN = A2;
int righB_PIN = A3;
int righ_PWM = 5;
int left_PWM = 6;
int trac1 = 11; //從車頭方向的最右邊開始排序
int trac2 = 10;
int trac3 = 9;
int trac4 = 8;
int trac5 = 7;
void setup()
{
//電機引腳初始化
Serial.begin(9600);
pinMode(leftA_PIN, OUTPUT);
pinMode(leftB_PIN, OUTPUT);
pinMode(righA_PIN, OUTPUT);
pinMode(righB_PIN, OUTPUT);
pinMode(righ_PWM, OUTPUT);
pinMode(left_PWM, OUTPUT);
pinMode(2, OUTPUT);
Serial.begin(9600);
Wire.begin();
sensor.init();
sensor.setTimeout(500);
sensor.startContinuous();
//尋跡模塊D0引腳初始化
pinMode(trac1, INPUT);
pinMode(trac2, INPUT);
pinMode(trac3, INPUT);
pinMode(trac4, INPUT);
pinMode(trac5, INPUT);
}
void loop() {
int buttonState = 0;//模式開關LOW-0循跡，HIGH-1藍牙
pinMode(4, INPUT); //模式開關
buttonState = digitalRead(4);
if (buttonState == HIGH) {
while(Serial.available())
{
char val=Serial.read();
if(val=='w'){
Serial.println("前進");
Serial.write("前進");
Serial.println();
motorRun(FORWARD);
}
else if(val=='s'){
Serial.println("后退");
Serial.write("后退");
Serial.println();
motorRun(BACKWARD);
}
else if(val=='d'){
Serial.println("右轉");
Serial.write("右轉");
Serial.println();
motorRun(TURNRIGHT);
delay(200);
motorRun(STOP);
}
else if(val=='a'){
Serial.println("左轉");
Serial.write("左轉");
Serial.println();
motorRun(TURNLEFT);
delay(200);
motorRun(STOP);
}
else if(val=='t'){
Serial.println("停止");
Serial.write("停止");
Serial.println();
motorRun(STOP);
}
else if(val=='l'){
Serial.println("原地左轉360");
Serial.write("原地左轉360");
Serial.println();
motorRun(TURNLEFT360);
motorRun(STOP);
}
else if(val=='r'){
Serial.println("原地右轉360");
Serial.write("原地右轉360");
Serial.println();
motorRun(TURNRIGHT360);
motorRun(STOP);
}
}
}
else
if (sensor.readRangeSingleMillimeters()<200)
{
digitalWrite(2,HIGH);
delay(100);
digitalWrite(2,LOW);
motorRun(BACKWARD);
delay(500);
}
else
tracing();
delay(5);
}
void motorRun(int cmd)
{
Serial.print(sensor.readRangeSingleMillimeters());
if (sensor.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
Serial.println();
switch(cmd){
case FORWARD:
Serial.println("FORWARD"); //輸出狀態
digitalWrite(leftA_PIN,LOW);
digitalWrite(leftB_PIN,HIGH);
digitalWrite(righA_PIN,LOW);
digitalWrite(righB_PIN,HIGH);
analogWrite(left_PWM,33);
analogWrite(righ_PWM,30); //左輪前進40 右輪前進40
break;
case BACKWARD:
Serial.println("BACKWARD"); //輸出狀態
digitalWrite(leftA_PIN,HIGH);
digitalWrite(leftB_PIN,LOW);
digitalWrite(righA_PIN,HIGH);
digitalWrite(righB_PIN,LOW);
analogWrite(left_PWM,25);
analogWrite(righ_PWM,25); //左輪后退40 右輪后退40
break;
case TURNLEFT:
Serial.println("TURN LEFT"); //輸出狀態
digitalWrite(leftA_PIN,LOW);
digitalWrite(leftB_PIN,HIGH);
digitalWrite(righA_PIN,LOW);
digitalWrite(righB_PIN,HIGH);
analogWrite(left_PWM,25);
analogWrite(righ_PWM,55); //左輪前進20 右輪前進45
delay(50);
break;
case STURNLEFT:
Serial.println("STURN LEFT"); //輸出狀態
digitalWrite(leftA_PIN,LOW);
digitalWrite(leftB_PIN,HIGH);
digitalWrite(righA_PIN,LOW);
digitalWrite(righB_PIN,HIGH);
analogWrite(left_PWM,10);
analogWrite(righ_PWM,35); //左輪前進30 右輪前進45
break;
case TURNRIGHT:
Serial.println("TURN RIGHT"); //輸出狀態
digitalWrite(leftA_PIN,LOW);
digitalWrite(leftB_PIN,HIGH);
digitalWrite(righA_PIN,LOW);
digitalWrite(righB_PIN,HIGH);
analogWrite(left_PWM,55);
analogWrite(righ_PWM,25); //左輪前進45 右輪前進20
break;
case STURNRIGHT:
Serial.println("STURN RIGHT"); //輸出狀態
digitalWrite(leftA_PIN,LOW);
digitalWrite(leftB_PIN,HIGH);
digitalWrite(righA_PIN,LOW);
digitalWrite(righB_PIN,HIGH);
analogWrite(left_PWM,35);
analogWrite(righ_PWM,10); //左輪前進45 右輪前進30
break;
case TURNRIGHT90:
Serial.println("TURN RIGHT 90"); //輸出狀態
digitalWrite(leftA_PIN,LOW); //HIGH 右90
digitalWrite(leftB_PIN,HIGH); //LOW
digitalWrite(righA_PIN,HIGH);
digitalWrite(righB_PIN,LOW);
analogWrite(left_PWM,40);
analogWrite(righ_PWM,20);
delay(590);
break;
case TURNLEFT90:
Serial.println("STURN LEFT 90"); //輸出狀態
digitalWrite(leftA_PIN,HIGH); //HIGH 左90
digitalWrite(leftB_PIN,LOW); //LOW
digitalWrite(righA_PIN,LOW);
digitalWrite(righB_PIN,HIGH);
analogWrite(left_PWM,20);
analogWrite(righ_PWM,40);
delay(520);
break;
case TURNLEFT360:
Serial.println("STURN LEFT 360"); //輸出狀態
digitalWrite(leftA_PIN,HIGH); //HIGH 左90
digitalWrite(leftB_PIN,LOW); //LOW
digitalWrite(righA_PIN,LOW);
digitalWrite(righB_PIN,HIGH);
analogWrite(left_PWM,40);
analogWrite(righ_PWM,40);
delay(2580);
motorRun(STOP);
break;
case TURNRIGHT360:
Serial.println("TURN RIGHT 360"); //輸出狀態
digitalWrite(leftA_PIN,LOW); //HIGH 右90
digitalWrite(leftB_PIN,HIGH); //LOW
digitalWrite(righA_PIN,HIGH);
digitalWrite(righB_PIN,LOW);
analogWrite(left_PWM,40);
analogWrite(righ_PWM,40);
delay(2760);
motorRun(STOP);
break;
default:
Serial.println("STOP"); //輸出狀態
digitalWrite(leftA_PIN,0);
digitalWrite(leftB_PIN,0); //左輪靜止不動
digitalWrite(righA_PIN,0);
digitalWrite(righB_PIN,0); //右輪靜止不動
}
}
void tracing()
{
int data[3];
data[0] = digitalRead(11);
data[1] = digitalRead(10);
data[2] = digitalRead(9);
data[3] = digitalRead(8);
data[4] = digitalRead(7);
Serial.print(data[0]);
Serial.print("---");
Serial.print(data[1]);
Serial.print("---");
Serial.print(data[2]);
Serial.print("---");
Serial.print(data[3]);
Serial.print("---");
Serial.println(data[4]);
if(!data[0]&&!data[1]&&!data[2]&&!data[3]&&!data[4]||data[0]&&data[1]&&!data[2]&&data[3]&&data[4])//00000直行
{
motorRun(FORWARD);
}
else if(data[0]&&!data[1]&&!data[2]&&!data[3]&&!data[4]) //10000左轉
{
motorRun(TURNLEFT);
}
else if(data[0]&&data[1]&&data[2]&&!data[3]&&!data[4]||data[0]&&data[1]&&!data[2]&&!data[3]&&!data[4]) //11100或11000左轉90度
{
motorRun(TURNLEFT90);
}
else if(!data[0]&&data[1]&&!data[2]&&!data[3]&&!data[4]) //01000小左轉
{
motorRun(STURNLEFT);
}
else if(!data[0]&&!data[1]&&!data[2]&&data[3]&&!data[4]) //00010小右轉
{
motorRun(STURNRIGHT);
}
else if(!data[0]&&!data[1]&&!data[2]&&!data[3]&&data[4]) //00001右轉
{
motorRun(TURNRIGHT);
}
else if(!data[0]&&!data[1]&&!data[2]&&!data[3]&&data[4]||!data[0]&&!data[1]&&!data[2]&&data[3]&&data[4]) //00111或00011右轉90度
{
motorRun(TURNRIGHT90);
}
else if(data[0]&&data[1]&&data[2]&&data[3]&&data[4]) //11111停止
{
motorRun(STOP);
}
}
轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/22220.html
標籤：硬件使用