這bug好難處理

目前有點小問題,目的是要讓超音波感測到障礙物時會馬達會停止,沒有障礙物時馬達會動作,但現在因為有加進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);*
    }