Login▼/회원가입BASIC4MCU | 질문게시판 | 아두이노 라인 following 질문
페이지 정보
작성자 금재 작성일2020-11-27 18:18 조회10,276회 댓글4건본문
/******************************************************************************************/
// MOTOR RIGHT ==> MOTOR DRIVER A
// MOTOR LEFT ==> MOTOR DRIVER B
// IR SENSOR RIGHT ==> A0 (SENSOR0)
// IR SENSOR LEFT ==> A1 (SENSOR1)
// ECHO ==> 8
// TRIG ==> 9
// TXD ==> 4
// RXD ==> 5
//
// Appinventor file : Robot_Controller_II_robotics_2020.aia
/******************************************************************************************/
#include <SoftwareSerial.h>
#include <Ultrasonic.h>
//ultrasonic sensor
char trig=9;
char echo=8;
int distance;
Ultrasonic ultrasonic(trig,echo);
// bluetooth communication
int blueRx = 4;
int blueTx = 5;
int flag=0;
unsigned char commandReadyFlag = 0;
unsigned char command, power;
SoftwareSerial mySerial(blueRx,blueTx); // 소프트웨어 시리얼통신 포트 생성
// sensor
#define BLACK 0
#define WHITE 1
// 회전 방향
#define CW 1 // clockwise
#define CCW 0 // counter-clockwise
// 왼쪽, 오른쪽 바퀴
#define MOTOR_LEFT 0
#define MOTOR_RIGHT 1
// motor settings
// 아두이노 PIN 설정
const byte PWMA = 3; // PWM control (speed) for motor A
const byte PWMB = 11; // PWM control (speed) for motor B
const byte DIRA = 12; // Direction control for motor A
const byte DIRB = 13; // Direction control for motor B
int sensor0, sensor1; // sensor reading 0 ~ 1023
int sensorLeft, sensorRight; // BLACK(0), WHITE(1)
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
mySerial.begin(9600);
setupArdumoto(); // Set all pins as outputs
}
void loop() {
//Ultrasonic sensor
int cutDistance = 5;
ultra();
// Bluetooth communication
communication();
if (command == 1)
{
if(distance > cutDistance){
// read sensor
readSensors();
// color finder
sensorRight = colorFinder(sensor0);
sensorLeft = colorFinder(sensor1);
// 로봇제어
robotControl(sensorLeft,sensorRight);
}
else
{
robotAvoidance();
}
}
else if(command == 2)
{
robotForward(100,100);
}
else if(command == 3)
{
robotRight(100,100);
}
else if(command == 4)
{
robotLeft(100,100);
}
else
{
robotStop();
}
}
//ultrasonic
void ultra(){
distance=ultrasonic.read();
/*
Serial.print("distance(CM): ");
Serial.println(distance);//distance in cm
delay(500);
*/
}
// setupArdumoto initialize all pins
void setupArdumoto()
{
// All pins should be setup as outputs:
pinMode(PWMA, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(DIRA, OUTPUT);
pinMode(DIRB, OUTPUT);
// Initialize all pins as low:
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, LOW);
digitalWrite(DIRA, LOW);
digitalWrite(DIRB, LOW);
}
void driveArdumoto(byte motor, byte dir, byte spd)
{
if(motor == MOTOR_RIGHT)
{
digitalWrite(DIRA, dir);
analogWrite(PWMA, spd);
}
else if(motor == MOTOR_LEFT)
{
digitalWrite(DIRB, dir);
analogWrite(PWMB, spd);
}
}
void robotForward(int powerLeft, int powerRight)
{
driveArdumoto(MOTOR_RIGHT,CCW,powerRight);
driveArdumoto(MOTOR_LEFT,CW,powerRight);
}
void robotBackward(int powerLeft, int powerRight)
{
driveArdumoto(MOTOR_RIGHT,CW,powerRight);
driveArdumoto(MOTOR_LEFT,CCW,powerRight);
}
void robotRight(int powerLeft, int powerRight)
{
driveArdumoto(MOTOR_RIGHT,CW,powerRight);
driveArdumoto(MOTOR_LEFT,CW,powerRight);
}
void robotLeft(int powerLeft, int powerRight)
{
driveArdumoto(MOTOR_RIGHT,CCW,powerRight);
driveArdumoto(MOTOR_LEFT,CCW,powerRight);
}
void robotStop()
{
stopArdumoto(MOTOR_LEFT);
stopArdumoto(MOTOR_RIGHT);
}
// stopArdumoto makes a motor stop
void stopArdumoto(byte motor)
{
driveArdumoto(motor, 0, 0);
}
// Read sensor measurements
void readSensors(){
sensor0 = 1023 - analogRead(A0); // sensor Right
sensor1 = 1023 - analogRead(A1); // sensor Left
sensorRight = sensor0;
sensorLeft = sensor1;
/*Serial.print("sensor R:");
Serial.println(sensorRight);
Serial.print("sensor L:");
Serial.println(sensorLeft);*/
}
// color finder
char colorFinder(int sensorValue) {
char color;
const int THRESHOLD = 800;
if(sensorValue > THRESHOLD)
color = WHITE;
else
color = BLACK;
return color;
}
////////////////////////////////////////////////////////////////
// 로봇제어 함수
/////////////////////////////////////////////////////////////////
void robotControl(int sensorLeft, int sensorRight){
if (sensorLeft == BLACK && sensorRight == BLACK){
robotForward(100,100);
}
else if (sensorLeft == BLACK && sensorRight == WHITE) {
robotLeft(100,100);
delay(30);
}
else if (sensorLeft == WHITE && sensorRight == BLACK) {
robotRight(100,100);
delay(30);
}
else{
if(flag==0){
robotForward(200,200);
flag=1;
} // flag=1;
else if(flag==1){
robotForward(200,200);
flag=2;
}
else {
robotStop();
}
}
}
void robotAvoidance()
{
robotRight(200,200); // 45도
delay(300);
robotForward(200,200); // 20 cm
delay(1100);
robotLeft(200,200); // 75 deg
delay(400);
robotForward(200,200); // 20 cm
delay(1400);
robotForward(200,200);
}
// Bluetooth communication
void communication(){
if (mySerial.available() >= 4){
//Serial.println("data arrived ");
unsigned char buffer[4];
// read data from buffer and save to array
for (char i=0; i<4; i++){
buffer[i] = mySerial.read();
}
// verify data
if ( buffer[0]== 255 && buffer[3] == 100) {
command = buffer[1];
power = buffer[2];
/* Serial.print("command:");
Serial.println(command);
Serial.print("power:");
Serial.println(power);*/
}
}
}
게시판에 라인을 따라가다 처음 이탈했을 때 정지하지 않고 다시 라인을 찾아 따라가다 두번째 이탈을 했을 때 멈추게 하는 질문을 보았습니다. 그렇다면 두번째 이탈 때도 멈추지 않고 세번째 이탈 했을 때 멈추게 하려면 아래의 코드가 맞나요??
else{
if(flag==0){
robotForward(200,200);
flag=1;
} // flag=1;
else if(flag==1){
robotForward(200,200);
flag=2;
}
else {
robotStop();
}
- BASIC4MCU 작성글 SNS에 공유하기
댓글 4
조회수 10,276master님의 댓글
master 작성일맞는 것 같습니다.
금재님의 댓글
금재 작성일안되는것 같습니다.. flag 값을 시리얼 프린트로 출력해봤는데 계속 2가 출력됩니다.
master님의 댓글
master
작성일
flag 가 0에서 2로 순간이동 할리는 없고
0->1->2 순차적으로 변경 됩니다.
즉, 0일 때와 1일 때가 이미 실행되었기 때문입니다.
예를들면 처음 조건을 만족해서 0->1이 되었다고 할 때
물리적으로 else 조건이 끝나지 못해서 연이어서 else가 몇번씩 실행 되었다고 볼 수 있겠죠
master님의 댓글
master 작성일
void robotControl(int sensorLeft, int sensorRight){
if (sensorLeft==BLACK && sensorRight==BLACK){ robotForward(100,100); }
else if(sensorLeft==BLACK && sensorRight==WHITE){ robotLeft(100,100); delay(30); }
else if(sensorLeft==WHITE && sensorRight==BLACK){ robotRight(100,100); delay(30); }
else{
if (flag==0){ flag=1; robotForward(200,200); } // flag 0->1
else if(flag==1){ flag=2; robotForward(200,200); } // flag 1->2
else { robotStop(); } // flag 2
}
}
질문 소스입니다.
위 코드를 조금 수정해드릴테니 잘 안되면 수정해서 완성시켜보세요
void robotControl(int sensorLeft, int sensorRight){
static char flagflag=0;
if (sensorLeft==BLACK && sensorRight==BLACK){ robotForward(100,100); flagflag=0; }
else if(sensorLeft==BLACK && sensorRight==WHITE){ robotLeft(100,100); flagflag=0; delay(30); }
else if(sensorLeft==WHITE && sensorRight==BLACK){ robotRight(100,100); flagflag=0; delay(30); }
else{
if(flagflag==0){ flagflag=1;
if (flag==0){ flag=1; robotForward(200,200); } // flag 0->1
else if(flag==1){ flag=2; robotForward(200,200); } // flag 1->2
else { robotStop(); } // flag 2
}
}
}
flagflag 변수를 추가했습니다.
else가 실행 될 때마다 flagflag가 1로 set 되고
else가 아닌 다른 조건을 실행 해야만 flagflag=0 이 되어서 다시 else를 실행 할 수 있게 됩니다.
즉, else 조건이 연속해서 만족되더라도 1회만 실행하도록 변경한 것입니다.