目前有點小問題,目的是要讓超音波感測到障礙物時會馬達會停止,沒有障礙物時馬達會動作,但現在因為有加進PWM去控制速度,但是這LOOP裡的速度會影響到SWITCH的速度,比方說當啟動FORWARD時速度是127但維持時間只維持很短,delay完就回到LOOP的速度191去,但拿掉那段後,速度維持的時間是持續的,但有點缺陷是感測到障礙物時停止,但當障礙物移走後就不會自行啟動了,請問要如何修正?
#include <SoftwareSerial.h>
#include <Wire.h>
#include <stdio.h>
int enablePin = 6;
int motorPin1 = 3;
int motorPin2 = 5;
int trig = 9 , echo = 8 ;
int beep = 12;
char x;
byte dis[6]; //distance package
SoftwareSerial BT(10, 11); // RX, TX
float distance = 0;
char dischar[4];
int i=0;
float Length()
{
float duration, distancetmp;
digitalWrite(trig, HIGH);
delayMicroseconds(1000);
digitalWrite(trig, LOW);
duration = pulseIn (echo, HIGH);
distancetmp = (duration / 2) / 29;
return distancetmp;
}
void setup()
{
// put your setup code here, to run once:
Serial.begin(9600);
BT.begin(9600);
pinMode(motorPin1, OUTPUT);
pinMode(motorPin2, OUTPUT);
pinMode(enablePin, OUTPUT);
pinMode(trig, OUTPUT);
pinMode(echo, INPUT);
pinMode(beep, OUTPUT);
digitalWrite(motorPin1, LOW);
digitalWrite(motorPin2, LOW);
digitalWrite(enablePin, LOW);
delay(1000);
}
void loop()
{
{
distance = Length();
if (distance < 20)
{
Serial.print("WARNING!");
Serial.print("Obstacle!!");
digitalWrite(beep,HIGH);
delay(500);
digitalWrite(beep,LOW);
analogWrite(enablePin, 0);
}
else
{
Serial.print("OK!");
digitalWrite(beep,LOW);
delay(500);
analogWrite(enablePin, 191);
}
Serial.print("distance = ");
Serial.print(distance);
Serial.println("cm");
delay(500);
}
int sendData = (int) (distance * 100); //times 100 and convert disance to integer
byte packet[3];
packet[0] = 97;
packet[1] = sendData / 256; //divides sendData to two 1 byte packets
packet[2] = sendData % 256;
if ( BT.available()>0)
{
if (BT.read() == 97)
{
for(int i = 0; i < 3; i++)
BT.write(packet*);*
-
}*
-
val = BT.read();*
-
}*
-
switch(x)*
-
{*
-
case 'F': // car forward*
-
forward();*
-
break;*
-
case 'B': // car back*
-
back();*
-
break;*
-
case 'S': // car stop*
-
motorstop();*
-
break; *
-
case 'H': // speed high*
-
motorhigh();*
-
break;*
-
case 'M': // speed mid*
-
motormedian();*
-
break; *
-
case 'L': // speed low*
-
motorlow();*
-
break; *
-
} *
}
void motorstop()
{ -
Serial.println("Stop!");*
-
digitalWrite(motorPin1, LOW);*
-
digitalWrite(motorPin2, LOW);*
-
analogWrite(enablePin, 0);*
-
delay(500);*
}
void forward()
{ -
Serial.println("Forward!");*
-
digitalWrite(motorPin1, HIGH);*
-
digitalWrite(motorPin2, LOW);*
-
analogWrite(enablePin, 127);*
-
delay(500);*
}
void back()
{ -
Serial.println("Back!");*
-
digitalWrite(motorPin1, LOW);*
-
digitalWrite(motorPin2, HIGH);*
-
analogWrite(enablePin, 127);*
-
delay(500);*
}
void motorhigh()
{ -
Serial.println("High!");*
-
digitalWrite(motorPin1, HIGH);*
-
digitalWrite(motorPin2, LOW);*
-
analogWrite(enablePin, 255);*
-
delay(500);*
}
void motormedian()
{ -
Serial.println("Median!");*
-
digitalWrite(motorPin1, HIGH);*
-
digitalWrite(motorPin2, LOW);*
-
analogWrite(enablePin, 150);*
-
delay(500);*
}
void motorlow()
{ -
Serial.println("Low!");*
-
digitalWrite(motorPin1, HIGH);*
-
digitalWrite(motorPin2, LOW);*
-
analogWrite(enablePin, 64);*
-
delay(500);*
}