허스키렌즈 태그 추적

/**

240824 전기모 연수 AI++팀 박혜림 vkand.tedu@gmail.com 수정 - 허스키렌즈 태그 트래킹(Tag Recognition). 수정내용 주석 //vk 

- 통신 :  

아두이노 MEGA - 블루투스 모듈(HC-06 or HM-10)  

         RX31 - TXD  

         TX31 - RXD  

- 조향 :  

아두이노 UNO - 모터 드라이버(점퍼는 사진 확인 후 적용)  

         p10 - enA 

         p11 - PUL(펄스)  

         p12 - DIR(디렉션)  

모터 드라이버 - 스텝모터(2번, 5번 미사용)  

         A+ - 모터 결선 좌측 1번째  

         A- - 모터 결선 좌측 3번째  

         B+ - 모터 결선 좌측 4번째  

         B- - 모터 결선 좌측 6번째  

모터 드라이버의 점퍼를 조정해 전류를 제어할 수 있음  

모터는 K6G50C 1:50 기어박스가 포함되어 있음  

점퍼는 off-on-off(200pulse/rev)로 세팅합니다. 

- 페달모드 :

아두이노 - 페달

     A0 - 노랑

     p7  - 스위치-전진

     p6  - 스위치-후진

- 구동 :  

아두이노 MEGA - 모터 드라이버  

전진후진(P8) - dir입력(모터 회전 방향)  

PWM출력(p9) - PWM입력(파워)  

*/

// 허스키렌즈를 사용하기 위해서 라이브러리 설치가 필요합니다. 

// 소프트웨어시리얼 핀은 아두이노 메가의 52, 53번을 사용합니다. 

#include <SoftwareSerial.h>

#include <HUSKYLENS.h>

HUSKYLENS huskylens;

SoftwareSerial mySerial(52, 53); // RX, TX

// 스텝 모터 제어

const int enA = 10;  // 구동 여부 결정

const int stepPin = 11; // 스텝 펄스

const int dirPinLR = 12;  // 좌우 회전

const int rst = 5; // 리셋, LOW 상태로 유지함

const int STEPS_PER_REV = 400; // 모터 2회전, 점퍼는 off-on-off로 세팅함(200pulse/rev)

int rotatePos = 10;

int rotateMid = 10;

int rotateLeftLimit = 7;

int rotateRightLimit = 13;

// 드라이브 모터 제어

const int DIR = 8; // 파워

const int PWM = 9; // 신호 1 

// 속력값 초기화

int speed = 40;

int c_speed = 0; //vk i->c_speed 속력 변화값

// 페달 제어_전진, 후진 스위치 센싱

int pedalF = 6;

boolean pedalFVal = 0;

int pedalB = 7;

boolean pedalBVal = 0;

const int ground = 4;

// 페달 제어_페달 센싱

const int pedalSensor = A0;

int pedalVal = 0;

// 수동 모드, 앱 제어모드 변경, 0은 앱제어모드, 1은 수동 모드

boolean modeState = 0; // vk 1->0

// 입력 문자, 입력 문자 백업

char cmd = "";

char cmdM = "s";

void setup() {

  //모니터링을 위한 시리얼 통신 설정

  Serial.begin(9600); // 시리얼 통신

  //mySerial은 소프트웨어 시리얼입니다. 

  mySerial.begin(9600); // 허스키렌즈와 시리얼 통신을 할 때 사용합니다. 

  //Serial3는 아두이노 메가를 사용할 때 사용하는 시리얼입니다. 아두이노 우노를 사용하면 주석처리 하세요.

  Serial3.begin(9600);           

  // 스텝모터 핀 모드 설정

  pinMode(dirPinLR,OUTPUT);

  pinMode(stepPin,OUTPUT); 

  pinMode(enA, OUTPUT);

  digitalWrite(enA, HIGH);

  pinMode(rst, OUTPUT);

  digitalWrite(rst, LOW);

  // 드라이브모터 핀 모드 설정

  pinMode(PWM,OUTPUT); 

  pinMode(DIR, OUTPUT);

  // 페달모드 전진 후진 신호

  pinMode(pedalF, INPUT_PULLUP);

  pinMode(pedalB, INPUT_PULLUP);

  pinMode(ground, OUTPUT);

  digitalWrite(ground, LOW);

  while (!huskylens.begin(mySerial)){

    Serial.println(F("\n Begin failed!"));

    Serial.println(F("1.Please recheck the \"Protocol Type\" in HUSKYLENS (General Settings >> Protocol Type >> Serial I2C )")); // vk 9600->I2C

    Serial.println(F("2.Please recheck the connection."));

    delay(100);

  }

  Serial.println("\n Huskylens Go-Kart is Ready!");

  delay(1000);

}

void swm_ccw_R() {  //vk left()->swm_ccw_R() Steering Wheel Motor CounterClockWise, Right Turn? 조향 모터 회전방향과 스티어링 휠 회전방향 반전?

  // 조향 모터가 '반시계방향'으로 회전하도록 신호부여

  digitalWrite(dirPinLR,LOW);  

  if (rotatePos > rotateLeftLimit) {

    // 1000마이크로초 주기로 모터 축이 1.5회전하는 코드

    // 1:50 기어박스 내장되어 있으므로, 모터 1회전에 바퀴 7.2도 회전함

    // 따라서, 모터가 1.5회전하면 바퀴가 10.8도 회전함

    for(int x = 0; x < STEPS_PER_REV*0.3; x++) {

      digitalWrite(enA,HIGH);

      digitalWrite(stepPin,HIGH);

      delayMicroseconds(500);

      digitalWrite(stepPin,LOW);

      delayMicroseconds(500); 

    }

    rotatePos = rotatePos - 1;

  } else {

    rotatePos = rotateLeftLimit;

  }

  Serial.print("\t\t rotPos: "+rotatePos);

}

void swm_cw_L() {  //vk right()->swm_cw_L() Steering Wheel Motor ClockWise, Left Turn? 조향 모터 회전방향과 스티어링 휠 회전방향 반전?

  // 조향 모터가 '시계방향'으로 회전하도록 신호부여

  digitalWrite(dirPinLR,HIGH); 

  if (rotatePos < rotateRightLimit) {

    for(int x = 0; x < STEPS_PER_REV*0.3; x++) {

      digitalWrite(enA,HIGH);

      digitalWrite(stepPin,HIGH);

      delayMicroseconds(500);

      digitalWrite(stepPin,LOW);

      delayMicroseconds(500); 

    }

    rotatePos = rotatePos + 1;

  } else {

    rotatePos = rotateRightLimit;

  }

  Serial.print("\t\t rotPos: "+rotatePos);

}

void forward() {

  Serial.print("\t\t forward");

  //드라이브 모터가 앞으로 회전하도록 신호부여

  digitalWrite(DIR,LOW); 

  analogWrite(PWM, c_speed);

  if (c_speed != speed) {

    for (c_speed = 0; c_speed < speed; c_speed = c_speed + 10) {

      analogWrite(PWM, c_speed);

      delay(100);

    }

  }

  if (rotateMid > rotatePos) {

    int j = rotateMid - rotatePos;

    for (int k = 0; k < j; k++) {

      swm_cw_L();

    }

  } else if (rotateMid == rotatePos) {

  } else if (rotateMid < rotatePos) {

    int j = rotatePos - rotateMid;

    for (int k = 0; k < j; k++) {

      swm_ccw_R();

    }

  }

}

void motorStop() {

  Serial.print("\t\t motorStop");

  digitalWrite(DIR,LOW);

  analogWrite(PWM, 0);

  delay(100);

  cmdM = 's';

}

void backward() {

  Serial.print("\t\t backward");

  ////드라이브 모터가 뒤로 회전하도록 신호부여

  digitalWrite(DIR,HIGH); 

  analogWrite(PWM, c_speed);

  if(c_speed != speed) {

    for (c_speed = 0; c_speed < speed; c_speed = c_speed + 10) {

      analogWrite(PWM, c_speed);

      delay(100);

    }

  }

}

void printResult(HUSKYLENSResult result){

    //vk 허스키렌즈 블록 정보

    if (result.command == COMMAND_RETURN_BLOCK){

      Serial.print(String()+F("\n Block \t xCenter: ")+result.xCenter+F("\t yCenter: ")+result.yCenter+F("\t width: ")+result.width+F("\t height: ")+result.height+F("\t ID: ")+result.ID);

      if(result.xCenter > 180) {//vk default:180 x축 중앙(160) 오른쪽. 실주행하며 수정필요.

        if(cmdM == 'x'){  //vk 이전cmd: 후진

          cmd = 'a';

          Serial.print(String() +"\t\t Set cmd: "+ cmd + F("\t B_LeftTurn"));   //vk Backward Right -> B_LeftTurn. 후진 좌회전 (스티어링휠 반시계 회전)

        } else {

          cmd = 'd';

          Serial.print(String() +"\t\t Set cmd: "+ cmd + F("\t RightTurn"));    //vk Right->RightTurn. 전진 우회전

        }  

      }

      if(result.xCenter < 140) { //vk default:140 x축 중앙(160) 왼쪽. 실주행하며 수정필요.

        if(cmdM == 'x'){ //vk cmdM: 후진

          cmd = 'd';

          Serial.print(String() +"\t\t Set cmd: "+ cmd + F("\t B_RightTurn"));   //vk left->B_RightTurn. 후진 우회전

        } else {

          cmd = 'a';

          Serial.print(String() +"\t\t Set cmd: "+ cmd + F("\t LeftTurn"));   //vk BL->leftTurn 전진 좌회전

        }

      }

       if(result.xCenter >= 140 && result.xCenter <=180 && result.yCenter >=100 && result.yCenter <= 140) { //vk default(140,180) x축 중앙(160) 부근

        cmd = 'w'; // vk s->w. 정지에서 직진으로 수정

        Serial.print(String() +"\t\t Set cmd: "+ cmd + F("\t Forward")); //vk left->stop->Forward 

      }

 //     if(result.width < 40) {   //vk default(W60, H60) 블록 크기가 설정값보다 작아졌을 때 전진 result.height값 삭제. 실주행하며 수정필요.

//        cmd = 'w';

 //       cmdM = 'w';

//        Serial.print(String() +"\t\t Set cmd: "+ cmd + F("\t Forward"));      

//      } 

      if(result.width > 80) {    //vk default(W90,H90) 블록 크기가 설정값보다 커졌을 때 후진. result.height값 삭제. 실주행하며 수정필요.

        cmd = 's';

        cmdM = 'x';

        Serial.print(String() +"\t\t Set cmd: "+ cmd + F("\t Backward"));  

      }

    }

    //vk 허스키렌즈 화살표 정보. 태그 트래킹에는 사용하지 않지만, 확장을 위해 남겨 둠.

    else if (result.command == COMMAND_RETURN_ARROW){

      Serial.print(String()+F("\n Arrow \t xOrigin: ")+result.xOrigin+F("\t yOrigin: ")+result.yOrigin+F("\t xTarget: ")+result.xTarget+F("\t yTarget: ")+result.yTarget+F("\t ID: ")+result.ID);

      cmd = 'w';      

      if(result.xTarget-result.xOrigin < 20) { //vk mod

        cmd = 'd';

        Serial.print(String() +"\t\t Set cmd: "+ cmd + F("\t A-RightTurn"));  

        if(result.xOrigin < 100) {

          cmd = 'w';

          Serial.print(String() +"\t\t Set cmd: "+ cmd + F("\t A-Forward"));   

        } 

      }

      else if(result.xTarget-result.xOrigin > -20) {

        cmd = 'a';

        Serial.print(String() +"\t\t Set cmd: "+ cmd + F("\t A-LeftTurn"));   

        if(result.xOrigin > 220) {

          cmd = 'w';

          Serial.print(String() +"\t\t Set cmd: "+ cmd + F("\t A-Forward"));   

        } 

      }

      else if(result.xOrigin < 120 && result.xTarget < 120) {

        cmd = 'a';

        Serial.print(String() +"\t\t Set cmd: "+ cmd + F("\t A-LeftTurn"));

      }

      else if(result.xOrigin > 200 && result.xTarget > 200) {

        cmd = 'd';

        Serial.print(String() +"\t\t Set cmd: "+ cmd + F("\t A-RightTurn"));

      }

      else {

        cmd = 'w';

        Serial.print(String() +"\t\t Set cmd: "+ cmd + F("\t A-Forward"));   

      } 

    }

    else{

      Serial.print("Object unknown!");

    }

}

void loop() {

  // 아두이노 메가: Serial3, 아두이노 우노: Serial3 -> mySerial 수정

  // 아두이노 메가: 스위치SW6 3번, 아두이노 우노: 스위치SW6 1번

  if (Serial3.available() ){        // 블루투스 통신에 데이터가 있을 경우

    cmd='i';     //vk m->i  cmd: m=수동모드, i=앱모드. modeState 변경

    cmd = Serial3.read();

//    cmd='i';     //vk 위치이동 m->i cmd: m=수동모드, i=앱모드. modeState 변경

    if (cmd == 'm') {

      modeState = 1;

      //vk Serial3.println("\n input 'm'");

      Serial3.println("Mode: Manual"); //vk

    }else if (cmd == 'i') {

      modeState = 0;

      //vk Serial3.println("\n input 'i'");

      Serial3.println("Mode: APP/Huskylens"); //vk

    } 

  }   

  //  modestate가 1이면 페달제어 모드로 수행

  if(modeState == 1) {       

    // 전진, 후진 스위치 값 저장

    pedalFVal = digitalRead(pedalF);

    pedalBVal = digitalRead(pedalB);

    // 페달값 센싱-매핑-한계범위설정

    pedalVal = analogRead(pedalSensor);

    pedalVal = map(pedalVal, 230, 850, 0, 255);

    pedalVal = constrain(pedalVal, 0, 255);

    // 페달 값 변화 시리얼 모니터 링

    Serial.print("\n pedalFVal: "+pedalFVal);

    Serial.print("\t\t pedalBVal: "+pedalBVal);

    Serial.println("\t\t pedalVal: "+pedalVal);

    // 페달 신호가 0이면 브레이킹

    if (pedalVal == 0) {

      digitalWrite(DIR,LOW);   

      analogWrite(PWM, 0);

      delay(100);

    }

    // 전진, 후진 스위치 값에 따른 페달 동작

    if (pedalFVal == 1 && pedalBVal == 0) {

      digitalWrite(DIR,LOW); 

      analogWrite(PWM, pedalVal);

      Serial.print("\t\t RRRR");

    } else if (pedalFVal == 1 && pedalBVal == 1) {

      digitalWrite(DIR,HIGH);

      analogWrite(PWM, pedalVal);

      Serial.print("\t\t FFFF");

    } else {

      digitalWrite(DIR,LOW);  

      analogWrite(PWM, 0);

      Serial.print("\t\t SSSS");

    }

  }

  // modestate가 0이면 허스키렌즈제어 모드로 수행

  if (modeState == 0) {

      // 아래는 허스키 렌즈로 제어할 때 사용

    if (!huskylens.request()) Serial3.println(F("\n Fail to request data from HUSKYLENS, recheck the connection!"));

    else if(!huskylens.isLearned()) Serial3.println(F("Nothing learned, press learn button on HUSKYLENS to learn one!"));

    else if(!huskylens.available()) Serial3.println(F("No block or arrow appears on the screen!"));

    else

    {

      while (huskylens.available())

      {

        HUSKYLENSResult result = huskylens.read();

        printResult(result);

        //vk Serial.print("\t\t cmdM: "+cmdM+"\t\t cmd: "+cmd);

        if (cmd == 'w' ){

            //vk Serial.print("\t\t cmd: "+cmd); 

            if(cmdM == 'w'){            //vk cmdM=w, cmd=w : 전진유지

              forward();

            } else {                    //vk cmdM!=w, cmd=w : 천천히 가속 전진

              c_speed = 0;

              forward();

            }

            cmdM = 'w';

        } else if ( cmd == 'x') {

            //vk Serial.print("\t\t cnd: "+cmd);

            if(cmdM == 'x') {            //vk cmdM=x, cmd=x : 후진유지

              backward();

            } else {                     //vk cmdM!=x, cmd=x : 천천히 가속 후진

              c_speed = 0;

              backward();

            }

            cmdM = 'x';

        } else if ( cmd == 'a' ) {       //vk a:좌회전

          swm_cw_L();

        } else if ( cmd == 'd' ) {       //vk d:우회전

          swm_ccw_R();

        } else if ( cmd == 's' ) {       //vk s:정지

          motorStop();

        } else if ( cmd == 'o' ) {       //vk o:느리게

          speed -= 30;

          if (speed < 30) {

            speed = 30;

          }

          // Serial.print("Speed Down! Current Speed is ");

          Serial.println("\t\t Speed-: "+speed); //vk

        } else if ( cmd == 'p' ) {       //vk p:빠르게

          speed += 30;

          if (speed > 250) {

            speed = 250;

          }

          // Serial.print("Speed Up! Current Speed is ");

          Serial.println("t\t Speed+: "+speed); //vk

        }

      }    

    }

  }

}