안녕하세요
현재 pixycam과 초음파 센서를 이용해서 선을 추적해서 따라가다가 초음파 센서로 장애물이 감지가 되면
 
정지하는 시스템을 구현 중인데 제대로 동작을 하지 않아 질문 드립니다.
 
 
 현재코드는 아래와 같은데  if(distance <10)이 코드를 넣으면 아예 동작을 안합니다. 
if(distance<10) 코드를 빼면 제대로 동작을 합니다.
 
그래서 초음파센서 코드만 실행해 봤는데 정상 작동을 하였습니다.
 
어디 코드를 어떻게 고쳐야 정상 작동 하나요? ㅠㅠ
 
#include <SPI.h>
#include <Pixy2.h>
#include <TPixy2.h>
#include <PIDLoop.h>
#include <Pixy2Line.h>
#include <ZumoMotors.h>
#include <SoftwareSerial.h>
 
 
 
 
 
// Zumo speeds, maximum allowed is 400
#define ZUMO_FAST        400
#define ZUMO_SLOW        200
#define X_CENTER         (pixy.frameWidth/2)
 
#define ledPin 5
#define trig 13
#define echo 12
 
Pixy2 pixy;
ZumoMotors motors;
 
SoftwareSerial mySerial(2, 3);
PIDLoop headingLoop(5000, 0, 0, false);
 
void setup() 
{
  Serial.begin(115200);
  mySerial.begin(9600);
  
  Serial.print("Starting...\n");
  motors.setLeftSpeed(0);
  motors.setRightSpeed(0);
 
  pixy.init();
  // Turn on both lamps, upper and lower for maximum exposure
  pixy.setLamp(1, 1);
  // change to the line_tracking program.  Note, changeProg can use partial strings, so for example,
  // you can change to the line_tracking program by calling changeProg("line") instead of the whole
  // string changeProg("line_tracking")
  pixy.changeProg("line_tracking");
 
  // look straight and down
  pixy.setServos(500, 1000);
 
  pinMode(ledPin, OUTPUT);
  pinMode(trig, OUTPUT);
  pinMode(echo, OUTPUT);
 
 
}
 
 
void loop()
{
  int8_t res;
  int32_t error; 
  int left, right;
  char buf[96];
  char code;
 
 digitalWrite(trig, LOW);
 digitalWrite(echo, LOW);
 delayMicroseconds(2);
 digitalWrite(trig, HIGH);
 delayMicroseconds(10);
 digitalWrite(trig, LOW);
 
  unsigned long duration = pulseIn(echo, HIGH);
  float distance = duration / 29.0 / 2.0;
 
 
  
 
 if(mySerial.available()){
  code=(mySerial.read());
  if(code == '1' or '2' or '3' or '0') {
    digitalWrite(ledPin, HIGH);
    delay(3000);
    digitalWrite(ledPin,LOW);
    }
 }
 
 
  if(distance <10)                   //초음파 조건문
{    motors.setLeftSpeed(0);   
    motors.setRightSpeed(0);
}
 
else{
 
  // Get latest data from Pixy, including main vector, new intersections and new barcodes.
  res = pixy.line.getMainFeatures();
 
 
  // If error or nothing detected, stop motors
  if (res<=0) // 스탑 부분
  {
    motors.setLeftSpeed(0);
    motors.setRightSpeed(0);
    Serial.print("stop ");
    Serial.println(res);
    return;
  }
 
  // We found the vector...
  if (res&LINE_VECTOR) //
  {
     // Calculate heading error with respect
 
// the part of the vector we're heading toward.
    error = (int32_t)pixy.line.vectors->m_x1 - (int32_t)X_CENTER;
 
    pixy.line.vectors->print();
 
    // Perform PID calcs on heading error.
    headingLoop.update(error);
 
    // separate heading into left and right wheel velocities.
    left = headingLoop.m_command;
    right = -headingLoop.m_command;
 
    // If vector is heading away from us (arrow pointing up), things are normal.
    if (pixy.line.vectors->m_y0 > pixy.line.vectors->m_y1)
    {
      // ... but slow down a little if intersection is present, so we don't miss it.
      if (pixy.line.vectors->m_flags&LINE_FLAG_INTERSECTION_PRESENT)
      {
        left += ZUMO_SLOW;
        right += ZUMO_SLOW;
      }
      else // otherwise, pedal to the metal!
      {
        left += ZUMO_FAST;
        right += ZUMO_FAST;
      }    
    }
    else  // If the vector is pointing down, or down-ish, we need to go backwards to follow.
    {
      left -= ZUMO_SLOW;
      right -= ZUMO_SLOW;  
    } 
    motors.setLeftSpeed(left);
    motors.setRightSpeed(right);
  }
 
  
 
  // If intersection, do nothing (we've already set the turn), but acknowledge with a beep.
  if (res&LINE_INTERSECTION)
  {
     if (code == 3)      {
      pixy.line.setNextTurn(45);
       pixy.line.intersections->print();
       }  // blue    90 degrees is a left turn 
     else if (code == 1) {
      pixy.line.setNextTurn(-45);
       pixy.line.intersections->print();
       } // red    -90 is a right turn
     else if (code == 2) {
      pixy.line.setNextTurn(0);
       pixy.line.intersections->print();
       }   // green    0 is a stat
     else if (code == 0) // stop
  { 
    motors.setLeftSpeed(0);
    motors.setRightSpeed(0);
    Serial.print("stop ");
    Serial.println(res);
    return;
   
      }
    }
  }
}