주요 수정 부분
// edit 박동배 선생님 yCenter값 추가
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
}
// edit 박동배 선생님 하단 주석처리
// 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'; // edit 박동배 선생님 학생 안전위해 후진을 정지로 수정
cmdM = 'x';
Serial.print(String() +"\\t\\t Set cmd: "+ cmd + F("\\t Backward"));
}
}
전체코드
/**
240824 전기모 연수 AI++팀 박혜림 [email protected] 수정 - 허스키렌즈 태그 트래킹(Tag Recognition). 수정내용 주석 //vk
240826 박동배 수정 - AI고카트 작동 확인. 학생 안전위해 후진 제외
- 통신 :
아두이노 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 전진 좌회전
}
}
// edit 박동배 선생님 yCenter값 추가
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
}
// edit 박동배 선생님
// 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'; // edit 박동배 선생님 학생 안전위해 후진을 정지로 수정
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 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
}
}
}
}
}