Go Down

Topic: 這bug好難處理 (Read 425 times) previous topic - next topic

xznm0216

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

Go Up