블루투스로 2륜 자동차를 조정하는 코드를 짜고 있는데 스마트폰 앱으로 연결후 데이터를 날리면 서보모터가 동작한 후에 블루투스 연결이 끊켜버립니다. 뭐가 문제인지 도저히 알수가 없네요..
#include <SoftwareSerial.h>
#include <Stepper.h>
#include <String.h>
#include "Servo.h"
Servo myservo;
Servo myservo1;
SoftwareSerial hm10(10, 11); //RX, TX 연결
#define trig 8
#define echo 7
bool flag = true;
float duration, distance;
int sonar_sum;
int dis = 2000;
char data[10] = { '0', '0', '0', '0', '0', '0', '0', '0', '0', '0' };
int motorspeed = 140;
byte leng;
void setup()
{
pinMode(trig, OUTPUT);
pinMode(echo, INPUT);
myservo.attach(5); //왼
myservo1.attach(3); //오
Serial.begin(9600);
hm10.begin(9600);
}
void loop()
{
// sonar();
char temp[100];
char mode[100];
//int val[100];
if (hm10.available())
{
leng = hm10.readBytes(temp, 20);
Serial.print("Input data Lenght : ");
Serial.println(leng);
delay(5);
for (int i = 0; i < leng; i++)
{
Serial.print(temp[i]);
Serial.println("tmp");
mode[i] = temp[i];
delay(5);
}
delay(5);
for (int i = 0; i < leng - 1; i += 2)
{
delay(5);
char chr = mode[i + 1];
int val;
val = chr - '0';
Serial.print(mode[i]);
Serial.print("명령어");
Serial.print("\t");
Serial.print(val);
Serial.println("값");
delay(5);
switch (mode[i])
{
case '7': mv_foward(val); break;
case '8': mv_left(val); break;
case '9': mv_right(val); break;
default:;
delay(5);
}
}
delay(5);
}
}
void sonar()
{
for (int i = 0; i < 10; i++) {
digitalWrite(trig, HIGH);
delay(10);
digitalWrite(trig, LOW);
// echoPin 이 HIGH를 유지한 시간을 저장 한다.
duration = pulseIn(echo, HIGH);
// HIGH 였을 때 시간(초음파가 보냈다가 다시 들어온 시간)을 가지고 거리를 계산 한다.
distance = ((float)(340 * duration) / 10000) / 2; // 딜레이 (작동중지 시간)
Serial.print(distance);
Serial.println("cm");
// 수정한 값을 출력
delay(100);
sonar_sum = +distance;
}
}
void mv_foward(int val)
{
sonar();
Serial.println(sonar_sum);
int sum = sonar_sum / 10;
if (sum > 10) {
Serial.println("move");
Serial.println("foward");
for (int i = 0; i < 10; i++) {
Serial.println("for");
if (val > i) {
Serial.println("if");
myservo.write(0);
myservo1.write(180);
delay(dis);
mv_stop();
delay(500);
}
else {
break;
}
}
}
else {
//소나걸림
Serial.println("소나스톱");
}
}
void mv_left(int val)
{
myservo.write(180);
myservo1.write(180);
delay(dis);
mv_stop();
}
void mv_right(int val)
{
myservo.write(0);
myservo1.write(0);
delay(dis);
mv_stop();
}
void mv_stop()
{
myservo.write(95);
myservo1.write(90);
}
|