자동차 방향,속도 제어 소스

자동차 방향,속도 제어 소스

#include <SoftwareSerial.h>

int dcl_pwn_pin  = 5; // 아날로그 출력 (DC 모터 왼쪽)

int dcl_in1_pin = 6; // 디지털 출력 (DC 모터 왼쪽)

int dcl_in2_pin = 7; // 디지털 출력 (DC 모터 왼쪽)

int dcr_pwn_pin  = 3; // 아날로그 출력 (DC 모터 오른쪽)

int dcr_in1_pin = 2; // 디지털 출력 (DC 모터 오른쪽)

int dcr_in2_pin = 4; // 디지털 출력 (DC 모터 오른쪽)

int drivingSpeedMin = 200; // 모터 최소 PWN

int drivingSpeedMax = 255; // 모터 최대 PWN

int drivingSpeedStep = 20; // 모터 PWN 단계

int drivingCount = 0;

int bluetoothTx = 13; // 디지털 PIN (블루투스 TX)

int bluetoothRx = 12; // 디지털 PIN (블루투스 RX)

SoftwareSerial bluetooth(bluetoothTx, bluetoothRx);

int waveSenceTrig_pin = 8; // 디지털 PIN (초음파 센서 Trig)

int waveSenceEcho_pin = 9; // 디지털 PIN (초음파 센서 Echo)

int ledPin = 11;    // 디지털 PIN (LED)

boolean ledStatus = false;

int ledCount = 0;

int speakerPin = 10; // 디지털 PIN (피에조 버저)

int speakerTone = 370; // 버저 주파수

boolean beepStatus = false;

int beepCount = 0;

int loopSpeed = 120; // 루프 1초간 반복속도

void dcMoterSetup() {

pinMode(dcl_pwn_pin, OUTPUT);

pinMode(dcl_in1_pin, OUTPUT);

pinMode(dcl_in2_pin, OUTPUT);

pinMode(dcr_pwn_pin, OUTPUT);

pinMode(dcr_in1_pin, OUTPUT);

pinMode(dcr_in2_pin, OUTPUT);

pinMode(speakerPin, OUTPUT);

pinMode(ledPin, OUTPUT);

}

void setMotor(char pos, char step)

{

int in1_pin, in2_pin;

if(pos == 'l') {

in1_pin = dcl_in1_pin;

in2_pin = dcl_in2_pin;

} else {

in1_pin = dcr_in1_pin;

in2_pin = dcr_in2_pin;

}

if(in1_pin <= 0 || in2_pin <= 0) return;

switch(step) {

case 'd':

digitalWrite(in1_pin, HIGH);

digitalWrite(in2_pin, LOW);

break;

case 'r':

digitalWrite(in1_pin, LOW);

digitalWrite(in2_pin, HIGH);

break;

default:

digitalWrite(in1_pin, LOW);

digitalWrite(in2_pin, LOW);

break;

}

}

void speedMotor(char pos, int speed) {

int pwn_pin;

int dcSpeed = 0;

if(pos == 'l') pwn_pin = dcl_pwn_pin;

else pwn_pin = dcr_pwn_pin;

if(pwn_pin <= 0) return;

if(speed > 0) {

dcSpeed = drivingSpeedMin + int(speed)*drivingSpeedStep;

if(dcSpeed > drivingSpeedMax) dcSpeed = drivingSpeedMax;

}

analogWrite(pwn_pin, dcSpeed);

}

void drivingMotorSpeed(char movePos) {

int lSpeed = 0;

int rSpeed = 0;

switch(movePos) {

case 'L':

case 'R':

case 'D':

case 'B':

lSpeed = 3;

rSpeed = 3;

break;

case 'l':

case 'r':

case 'd':

case 'b':

lSpeed = 1;

rSpeed = 1;

break;

case 'z':

case 'q':

lSpeed = 1;

rSpeed = 3;

break;

case 'c':

case 'e':

lSpeed = 3;

rSpeed = 1;

break;

}

speedMotor('l', lSpeed);

speedMotor('r', rSpeed);

}

void drivingMotorDirection(char movePos) {

char lStep = ' ';

char rStep = ' ';

switch(movePos) {

case 'D':

case 'd':

case 'q':

case 'e':

lStep = 'd';

rStep = 'd';

break;

case 'L':

case 'l':

lStep = 'r';

rStep = 'd';

break;

case 'R':

case 'r':

lStep = 'd';

rStep = 'r';

break;

case 'B':

case 'b':

case 'z':

case 'c':

lStep = 'r';

rStep = 'r';

break;

}

setMotor('l', lStep);

setMotor('r', rStep);

}

void drivingMotor(char movePos) {

drivingMotorSpeed(movePos);

drivingMotorDirection(movePos);

if(movePos != 'p') {

delay(30);

ledCount += 30;

beepCount += 30;

}

}

void speakerOnOff(long cm) {

if(!beepStatus) {

noTone(speakerPin);

return;

}

if(cm <= 20) {

int bi = 4;

if(cm <= 5) bi = 1;

else if(cm <= 10) bi = 2;

else if(cm <= 15) bi = 3;

boolean state = true;

if(int(beepCount / (loopSpeed/2)*bi) == 0) state = true;

else state = false;

if(state) tone(speakerPin, speakerTone);

else noTone(speakerPin);

if(beepCount >= (loopSpeed)*bi) beepCount = 0;

else beepCount++;

} else {

noTone(speakerPin);

beepCount = 0;

}

}

void ledOnOff() {

if(!ledStatus) {

digitalWrite(ledPin, LOW);

return;

}

int state = LOW;

if(int(ledCount / (loopSpeed/2)) == 0) state = HIGH;

digitalWrite(ledPin, state);

if(ledCount >= loopSpeed) ledCount = 0;

else ledCount++;

}

void waveSence() {

digitalWrite(waveSenceTrig_pin,HIGH);

delayMicroseconds(10);

digitalWrite(waveSenceTrig_pin,LOW);

long duration = pulseIn(waveSenceEcho_pin,HIGH);

long cm = microsecondsToCentimeters(duration);

speakerOnOff(cm);

}

long microsecondsToCentimeters(long microseconds)

{

  // The speed of sound is 340 m/s or 29 microseconds per centimeter.

  // The ping travels out and back, so to find the distance of the

  // object we take half of the distance travelled.

  return microseconds / 29.0 / 2.0;

}

void setup()

{

pinMode(dcl_pwn_pin, OUTPUT);

pinMode(dcl_in1_pin, OUTPUT);

pinMode(dcl_in2_pin, OUTPUT);

pinMode(dcr_pwn_pin, OUTPUT);

pinMode(dcr_in1_pin, OUTPUT);

pinMode(dcr_in2_pin, OUTPUT);

pinMode(speakerPin, OUTPUT);

pinMode(ledPin, OUTPUT);

pinMode(waveSenceTrig_pin, OUTPUT);

pinMode(waveSenceEcho_pin, INPUT);

beepCount = 0;

ledCount = 0;

drivingCount = 0;

Serial.begin(9600);

bluetooth.begin(9600);

}

 

void loop()

{

if(bluetooth.available()) {

char toSend = (char)bluetooth.read();

drivingMotor(toSend);

drivingCount = 0;

switch(toSend) {

case 'o':

beepStatus = true;

break;

case 'x':

beepStatus = false;

break;

case 'y':

ledStatus = true;

break;

case 'n':

ledStatus = false;

break;

}

}

waveSence(); // 거리 측정 하기

ledOnOff();

if(drivingCount > loopSpeed*4) {

speedMotor('l', '0');

setMotor('l', 'p');

speedMotor('r', '0');

setMotor('r', 'p');

}

drivingCount++;

}