AI_Car_HL_vDS_v2 02.ino

2024.08.27 이후에도 계속 수정 중.

/**
  240824 전기모 연수 AI++팀 박혜림 [email protected] 수정 - 허스키렌즈 태그 트래킹(Tag Recognition). 수정내용 주석 //vk
  240827 전기모 동신중 박동배 업그레이드 고카트

  - 통신 :
  아두이노 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";

#define sensorx 50 //슬라이드 스위치
int statex;

void setup() {
  //모니터링을 위한 시리얼 통신 설정
  
  pinMode(22, OUTPUT);  //led

  pinMode(sensorx, INPUT); //슬라이드 스위치
  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(String() +F("\\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(String() +F("\\t\\t rotPos: ") + rotatePos);
}

void forward() {
  Serial.print(String() +F("\\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(String() +"\\t\\t motorStop");

  digitalWrite(DIR, LOW);

  analogWrite(PWM, 0);
  delay(100);

  cmdM = 's';
}

void backward() {
  Serial.print(String() +"\\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) 오른쪽. 실주행하며 수정필요.
      
        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) 왼쪽. 실주행하며 수정필요.
      
        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 >= 80 && result.yCenter <= 180 ) { //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 > 50 ) {   //vk default(W90,H90) 블록 크기가 설정값보다 커졌을 때 후진. result.height값 삭제. 실주행하며 수정필요.
      cmd = 's';
 //     cmdM = 'x';
      Serial.print(String() + "\\t\\t Set cmd: " + cmd + F("\\t stop"));
    }
  }

  else {
    Serial.print("Object unknown!");
  }
}

void loop() {

  // 아두이노 메가: Serial3, 아두이노 우노: Serial3 -> mySerial 수정
  // 아두이노 메가: 스위치SW6 3번, 아두이노 우노: 스위치SW6 1번

  statex = digitalRead(sensorx);
  //  modestate가 1이면 페달제어 모드로 수행
  if (statex == 1) {
    digitalWrite(22, HIGH); //led
    digitalWrite(enA, LOW); //스텝모터 작동 OFF

    // 전진, 후진 스위치 값 저장
    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");
    }
  } else {
    digitalWrite(22, LOW); //led 꺼라
    digitalWrite(enA, HIGH); //스텝모터 작동 ON
    // 아래는 허스키 렌즈로 제어할 때 사용
    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 cmd: "+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
        }
      }
    }
  }
}