7. 레이저 센서 구현하기 , 초음파 센서 합치기

이제 물품을 레이저가 인식했을 때 서보모터를 움직이자

import sys
import numpy as np
import cv2

import serial
import time

cap = cv2.VideoCapture(0)
if not cap.isOpened():
    print("열리지 않아요")
    sys.exit()

ser = serial.Serial('/dev/ttyACM0', 9600)
count = 0
de_count = 0 
radius_list = []

munjang = "not detect"
R = 255
G = 255
B = 255

new_circle_detect = True

while True:
    ret, frame = cap.read()
    if not ret:
        break

    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    blr = cv2.GaussianBlur(gray, (0, 0), 1.0)
    # 이미지의 잡음을 제거한다.


    # 실질적인 허프변환이 시작되는 부분
    circles = cv2.HoughCircles(blr, cv2.HOUGH_GRADIENT, 1, 50, param1=120, param2=40, minRadius=10, maxRadius=80)
    # 반지름과 threshold를 조절하면서 확인해볼 분이다.

    
    dst = frame.copy()
    # 이미지를 복사해서 dst에 저장한다.
    
    cv2.imshow('img', frame)
    
    # 원을 검출할 때 실행된다.
    if circles is not None:
        cv2.putText(dst, munjang, org=(0, 30), 
        fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1, 
        color=(R,G,B),thickness=1, lineType=cv2.LINE_AA)
        cx, cy, radius = circles[0][0]
        cv2.circle(dst, (int(cx), int(cy)), int(radius), (255, 0, 0), 2, cv2.LINE_AA)
        cv2.putText(dst, str(radius), org=(int(cx), int(cy)), 
        fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1, 
        color=(0,0,255),thickness=3, lineType=cv2.LINE_AA)
        radius_list.append(radius)
        count += 1
        print(count)
        
        if count == 11 and new_circle_detect == True:
            val_radius = np.average(radius_list)
            new_circle_detect = False
            if np.average(radius_list) >= 25 and np.average(radius_list) <= 31:
                munjang = "pass  (" + str(np.average(radius_list)) + ")"
                val = "1/"
                #val = "pass," + str(np.average(radius_list))
                R = 255
                G = 0
                B = 255
            else:
                munjang = "fail  (" + str(np.average(radius_list)) + ")"
                val = "0/"
                #val = "fail," + str(np.average(radius_list))
                R = 0
                G = 0
                B = 255

            val = val.encode('utf-8')
            ser.write(val)
            radius_list.clear()
            count = 0   
        de_count = 0

        time.sleep(0.1)
    
    else:
        cv2.putText(dst, munjang, org=(0, 30), 
        fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1, 
        color=(R,G,B),thickness=1, lineType=cv2.LINE_AA)
        new_circle_detect = True
        
        de_count += 1
        if de_count == 3:
            munjang = "not detect"
            R = 255
            G = 255
            B = 255
            de_count = 0
            count = 0
            radius_list.clear()
            
        time.sleep(0.1)
    

    cv2.imshow('img', dst)
    key = cv2.waitKey(1)

    if key == 27:
        break
    


cap.release()
cv2.destroyAllWindows()

/*
String come;

#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x3f, 16, 2);


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  lcd.begin();
  lcd.backlight();
}

void loop() {

  while(Serial.available()){
    String come = Serial.readStringUntil('/');
    Serial.println(come);
    lcd.begin();
    lcd.setCursor(0, 0);
    lcd.print(come);
    delay(500);
  }
}
*/

#include <Wire.h>
#include <Servo.h>

#define servoPin1 9
#define servoPin2 10
Servo servo1;
Servo servo2;

int angle=0;
String come[2];

int dis_new = 0;

int sunseo = 0;
int act = 0;


//TOF10120 모듈의 I2C 주소
#define TOF10120 0x52 //0xA4 > 0x52(7Bit)  

void SensorRead(unsigned char addr,unsigned char* datbuf,unsigned char cnt) 
{
  unsigned short result=0; 
  Wire.beginTransmission(TOF10120);
  Wire.write(byte(addr));  
  Wire.endTransmission();
  Wire.requestFrom(TOF10120, (int)cnt);
  if (cnt <= Wire.available()) {
    *datbuf++ = Wire.read();
    *datbuf++ = Wire.read();
  }
}

int ReadDistance()
{      
    unsigned short distance;
    unsigned char i2c_rx_buf[2];
    //모듈의 0x00번지부터 2바이트 읽어오기
    SensorRead(0x00,i2c_rx_buf,2);
    
    //읽어온 두 바이트를 변수 하나로 합침
    // i2c_rx_buf[0] : 상위 바이트
    // i2c_rx_buf[1] : 하위 바이트
    distance=i2c_rx_buf[0];
    distance=distance<<8;
    distance|=i2c_rx_buf[1];
 
    //300ms 대기
    delay(100); 
    return distance;
}


void setup() {
  //I2C 라이브러리 시작
  Wire.begin();


  // put your setup code here, to run once:
  Serial.begin(9600);

  servo1.attach(servoPin1);
  servo2.attach(servoPin2);

  servo1.write(90);
  servo2.write(90);
}

void loop() {
  //put your main code here, to run repeatedly:
  // To the Left, classification
  int x=ReadDistance();
    //시리얼 모니터에 출력
  dis_new = x;


  while(Serial.available()){
    //모듈에서 거리 데이터 읽어보기
    come[sunseo] = Serial.readStringUntil('/');
    sunseo++;
    if(sunseo == 2){
      sunseo = 0;
    }
  }

  

  if(come[act] == "1" && dis_new <= 50){
      servo1.write(120);
      servo2.write(115);

      come[act] = "X";
      
      act++;
      if(act == 2){
        act = 0;
      }
      delay(100);

    }
    else if(come[act] == "0" && dis_new <= 50){
      servo1.write(75);
      servo2.write(75);

      come[act] = "X";
      act++;
      if(act == 2){
        act = 0;
      }      
      delay(100);
    }


}

 

 

 

 

 

이번에는 초음파 센서를 넣었다

#define TRIG 9
#define ECHO 8
bool is_start = 0;
bool dont_move = 0;
int count = 0;
int warning = 0;
int speed = 255/4;
long distance;
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(4, 1);
  pinMode(5, 1);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
}
void loop() {
  if(count < 10000){
    long duration;
    digitalWrite(TRIG, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG, LOW);
    duration = pulseIn(ECHO, HIGH); // 초음파를 받은 시간 (LOW 에서 HIGH 로 )
    distance = duration * 17 / 1000; // cm 로 환산 (34000 / 10000000 /2 를 간단하게)
    Serial.print("\nDIstance : ");
    Serial.print(distance);
    Serial.println(" cm");
  if(distance < 5)
    warning++;
  else
    warning = 0;
  count = 0;
  }
  count++;
  if(warning < 3 && dont_move == 0){
    if (is_start == 0) {
      digitalWrite(4, 1); // 5V : +
      digitalWrite(5, 0); // GND (0V) :-
      analogWrite(3, 255);
      delay(100);
      is_start = 1;
    }
    // put your main code here, to run repeatedly:
    digitalWrite(4, 1); // 5V : +
    digitalWrite(5, 0); // GND (0V) :-
    analogWrite(3, speed);
    Serial.print("current speed : ");
    Serial.println(speed);
  }
  else{
    dont_move = 1;
    if(speed > 5){
      Serial.print("stopping speed : ");
      Serial.println(speed);
      speed = speed - 5;
    }
    else{
      Serial.print("stop : ");
      Serial.println(speed);
      speed = 0;
      delay(1000);
      if(distance >= 5){
        dont_move = 0;
        warning = 0;
        is_start = 0;
        speed = 255/4;
      }
    }
    analogWrite(3, speed);
  }
}

기동전압이 부족해서

기동이 안된다.

현재는 1.5V 4개를 이어놓은 상태라 6V의 전압이 인가되고 있었다.

 

그래서 9V 건전지를 가져와서 

인가하니 속도가 미쳤다..