DC모터 테스트 코드

지난번 DC 모터 테스트에서 사용한 코드는 아래와 같다.

추가적인 작업으로 처음 움직일 때 가속하도록 하여 미끄러지지 않도록 하였다. 물론 전반적인 최종값들은 무게에 따라 조정해야겠지만 현재는 그나마 덜 미끄러지면서 진행된다.

또한 추가적으로 양쪽 IR센서의 조정을 진행했다. 센서인식 타이밍이 너무 짧아서 양쪽 동시 인식이 되지 않아 한쪽 인식한 후 다른 쪽의 인식에 딜레이를 주니 그나마 인식이 잘된다.

음... 아직 추가할 것도 많은데 점점 길어진다.

#include <SoftwareSerial.h>
 
#define irPin_L A0 // ir 센서
#define irPin_R A1
// #define usPin A2 // 초음파 센서
 
#define motor_LF 5  //  PWM으로 속도제어
#define motor_LB 6
#define motor_RF 9
#define motor_RB 10
 
// #define servoM_1 7
// #define servoM_2 8
// #define servoM_3 11
// #define servoM_4 12
 
#define bt_RX 2
#define bt_TX 3
SoftwareSerial _bluetooth(bt_RX, bt_TX);
 
bool _state = true;
String _inString = "";
 
bool _moveState = false;
bool _autoMode = false;
int _mPower = 150; // 100 ~ 254 // 최저부터 최고 // default = 0 or 250
int _mPowerAccel = 0;
float _mMoveTime = 0;
int _t = 0;
 
void setup()
{
  pinMode(irPin_L, INPUT);
  pinMode(irPin_R, INPUT);
  pinMode(motor_LF, OUTPUT);
  pinMode(motor_LB, OUTPUT);
  pinMode(motor_RF, OUTPUT);
  pinMode(motor_RB, OUTPUT);
 
  MotorActive(0, 0, 0, 0);
 
  _bluetooth.begin(9600);
  //Serial.begin(9600);
  Serial.println("Start~~");
  delay(1000);
}
 
///////////////////////////////////////////////////////////////////////
void loop()
{
  if(_bluetooth.available() && _state)
//  if(Serial.available() > 0 && _state)
  {
//    _inString = Serial.readString(); // mvf254,5 -> 파워,이동시간(초)
    _inString = _bluetooth.readString();
 
    Serial.println(_inString);
 
    int _index = _inString.indexOf(',');
    int _stringLength = _inString.length();
    String _moveStr = _inString.substring(3, _index);
    String _speedStr = _inString.substring(_index + 1, _stringLength);
    _inString = _inString.substring(0, 3);
 
    _mPower = _moveStr.toInt();
    _mPowerAccel = _mPower;
    _mPower = 80; // 가속하기 위한 초기값
    
    _mMoveTime = _speedStr.toFloat()*1000;
    
    _t = 0;    
    _moveState = true;
  }
 
  if(_t > _mMoveTime && _moveState)
    Stop();
 
  if(_moveState)
  {
    if(_mPower < _mPowerAccel)  //  가속값
    {
      _mPower += 1;
      delay(5);
    }
    
    if(!_autoMode)
    {
      if(_inString == "mvf")      // 전진
        MotorActive(0, 0, _mPower, _mPower);
      else if(_inString == "mvb") // 후진
        MotorActive(_mPower, _mPower, 0, 0);
      else if(_inString == "anr") // 우회전
        MotorActive(0, _mPower, _mPower, 0);
      else if(_inString == "anl") // 좌회전
        MotorActive(_mPower, 0, 0, _mPower);
    }
    
  if(_inString == "stp")
      Stop();
  else if(_inString == "ato") // 간단한 오토모드 테스트
    _autoMode = true;
 
  // 이동중이면 센서 작동
  float _irL = analogRead(irPin_L);
  float _irR = analogRead(irPin_R);
 
  if(_autoMode) // 일단 오토모드일때만 센서반응 적용
  {
    /* 동시감지가 양쪽 타이밍에 오차범위가 현저히 적어 안된다. 1.10 수정
    if(_irL < 100 && _irR < 100)      // 양쪽이 동시 감지되었을때
      SensorDetect(2);
    else if(_irL < 100 && _irR > 100) // 왼쪽이 감지되었을때
      SensorDetect(0);
    else if(_irL > 100 && _irR < 100) // 오른쪽이 감지되었을때
      SensorDetect(1); //Stop();
    */
 
     if(_irL < 150) // 한쪽 선인식한 다음 0.2초 후 반대쪽 인식되었을때 양쪽인식
     {
        delay(200);
        if(analogRead(irPin_R) < 100)
          SensorDetect(2);
        else
          SensorDetect(0);
     }
     else if(_irR < 100)
     {
        delay(200);
        if(analogRead(irPin_L) < 100)
          SensorDetect(2);
        else
          SensorDetect(1); 
     }
  } 
  else 
  {
    if(_irL < 100 || _irR < 100)
    Stop();
  }
  
  _t += 10;
  }
  
  delay(10); // 1초 = 1000ms
 
  if(_autoMode)
    MotorActive(0, 0, _mPower, _mPower);    // 적정 속도로 전진
        
}
///////////////////////////////////////////////////////////////////////
 
void Stop() // 정지 초기화, 다음 명령어 대기함
{ 
  MotorActive(0, 0, 0, 0);
  _moveState = false;
  _autoMode = false;
  _inString = "";
  _t = 0;
  Serial.println("stop");
}
 
void MotorActive(int LF, int RF, int LB, int RB)
{
  analogWrite(motor_LF, LF);
  analogWrite(motor_RF, RF);
  analogWrite(motor_LB, LB);
  analogWrite(motor_RB, RB);
}
 
void SensorDetect(int _dir)  //  센서 감지시 동작
{
  _state = false; // 입력수신 보류
  _mPower = 130;  // 고정값으로 처리
 
  Stop();                       // 일단 정지;
  delay(1000);
  
  MotorAccel(_mPower, _mPower, 0, 0); //MotorActive(130, 130, 0, 0);  // 살짝 뒤로 이동
  delay(800);
 
  if(_dir == 0)
    MotorAccel(0, _mPower, 0, 0);
  else if(_dir == 1)
    MotorAccel(_mPower, 0, 0, 0);
  else if(_dir == 2)            // 센서 양쪽 유효면 뒤로 회전
    MotorAccel(_mPower, 0, 0, _mPower);
    
  delay(800);
  
  MotorActive(0, 0, 0, 0);
  delay(1500);
  
  _autoMode = true;
  _moveState = true;
  _state = true; //  입력수신 시작
}
 
void MotorAccel(int LF, int RF, int LB, int RB)   // 센서감지시 반응 가속값
{
  _mPower = 80; // 시작값  
  while(_mPower < 130) // 목표 속도로 가속
  {
    _mPower += 1;
    MotorActive(LF, RF, LB, RB);
    delay(10);
  }
}