로봇제작 - 아두이노 테스트 코드 수정

아두이노 로봇암 동작의 테스트 코드를 좀 수정했다. 

반복되는 것을 배열로 정리하고 좀 더 수정하기 편하게 보완하였다. 이전 것은 테스트 명령어를 반복적으로 주면 오류가 있었다.  이것도 함께 보완하였다.

이참에 앱에서는 간단한 것은 명령어 함수로 보내고 물건 집는 것과 제자리 돌아가기 등의 정해진 동작은 아두이노에 미리 만들어서 넣으면 오류 확률이 더욱 줄 것 같다.

그리고 동작에 대한 입력 값이 있을 경우에만 서보모터에 연결되어서 잡음이 들리지 않는다. 모터 또한 전력이 들어가지 않아서 배터리도 절약할 수 있지 않을까 한다.

#include <Servo.h>
 
Servo servoM_1;
Servo servoM_2;
Servo servoM_3;
Servo servoM_4;
 
// 각도 제한값
const int _f1 = 15;   // 최소각
const int _f2 = 35;   // 최소각
const int _f3 = 0;    // 전체 범위각으로 제외함
const int _f4 = 135;  // 최소각
 
bool _state = true;
bool _activeState = false;
String _inString = "";
String _inStr = "";
String _inMod = "";
 
bool _armState = false;
bool _servoState = false;
int _mPower = 0;
int _mPowerTarget = 0;
float _mSpeedTime = 0;
 
bool _testState = false;
int _testNum = 0;
int _actionState = 0; // 0:대기, 1:작동중, 2:작동완료
 
String _actions[] ={"gx070n10", "gz080n10", "gx130n10", "gc060n10", "gv140n10", "gz140n10", // 잡기
                    "gv176n10", "gz080n10", "gx090n10", "gz015n10", "gc110n10", "gx035n10" };  // 복귀
 
void setup() 
{
  ServoConnect(false); // 리셋 on / off
 
  Serial.begin(9600);
  Serial.setTimeout(40);
  Serial.println("Start~");
  delay(500);
}
 
// test code
void TestStart()
{
  _inString = TestAction(_testNum);    // 현재 순서와 동작 각 가져오기
      
  _inMod = _inString.substring(0, 1);
  _inStr = _inString.substring(1, 2);
 
  int _index = _inString.indexOf('n');
  int _stringLength = _inString.length();
  String _moveStr = _inString.substring(2, _index);   // 파워
  String _speedStr = _inString.substring(_index+1, _stringLength);  // 이동값(시간속도)
                
  _mPowerTarget = _moveStr.toInt();       // 입력 받은 목표각도
  _mSpeedTime = _speedStr.toFloat()*10;   // 입력 받은 속도값  
       
  if(_inStr == "z")      _mPower = servoM_1.read();
  else if(_inStr == "x") _mPower = servoM_2.read();
  else if(_inStr == "c") _mPower = servoM_3.read();
  else if(_inStr == "v") _mPower = servoM_4.read();
        
  _armState = true;
}
 
void loop() 
{
  delay(20);
  
  if(Serial.available() > 0 && _state)
  {
    _inString = Serial.readString(); // mf254n9 (m:이동, f:방향, 254:파워, n:구분자, 5:시간)
                                     // gz045n9 (g:관절번호, 060:각도, n:구분자, 5:속도)
                                     // z, x, c, v 관절번호 순서
                                     // tg050n10 > 서보테스트모드, gf050n10 > 서보off
    Serial.println(_inString);
 
    _inMod = _inString.substring(0, 1); // 첫째문자 : 모드(이동 or 관절)
    _inStr = _inString.substring(1, 2); // 둘째문자 : 방향 or 관절 > f로 오면 서보off
 
    if(_inMod == "t" && !_testState)
    {
      if(!_servoState)  // 서보가 비활성화일때만 활성화 함
        ServoConnect(true);
        
       _actionState = 0;
       _testState = true;
       Serial.println("Activate 1");
       TestStart();
       return;
    }
    else if (_inMod == "f")
    {
      _testState = false;
      ServoConnect(false);
    }
 
  }
 
  if(_testState && _actionState == 2)
    TestStart();
 
  if(_armState) // 동작 시작 > 우선 한 관절씩만 움직이도록 하자
  {
    if(_inStr == "z")
      ArmActive(servoM_1, _f1, _mPowerTarget, _mSpeedTime);
    else if(_inStr == "x")
      ArmActive(servoM_2, _f2, _mPowerTarget, _mSpeedTime);
    else if(_inStr == "c")
      ArmActive(servoM_3, _f3, _mPowerTarget, _mSpeedTime);
    else if(_inStr == "v")
      ArmActive(servoM_4, _f4, _mPowerTarget, _mSpeedTime);
  }  
}
 
 
void ServoConnect(bool mode)  // 서보모터를 사용할때만 연결하기
{
  if(mode)
  {
    // 서보 각 핀 배치
    servoM_1.attach(7);
    servoM_2.attach(8);
    servoM_3.attach(11);
    servoM_4.attach(12);
  
    // 서보 리셋으로 최초 기본 값
    servoM_1.write(_f1);  // 각도가 클수록 위쪽으로
    servoM_2.write(_f2);  // 각도가 클수록 위쪽으로
    servoM_3.write(110);   // 기본 중간각, 각도가 클수록 아래쪽으로 
    servoM_4.write(176);  // 집게 : 135도 ~ 176도 유효각도
  }
  else
  {
    servoM_1.detach();
    servoM_2.detach();
    servoM_3.detach();
    servoM_4.detach();
  }
 
  _actionState = 0;
  _servoState = mode;
  delay(200);
}
 
 
void ArmActive(Servo _servo, int _const, int _target, int _speed)
{
  _actionState = 1;
  if(_servo.read() < _target && _const+1 < _target)
  {
    _mPower += 1;
    _servo.write(_mPower);
    delay(_speed);
  }
  else if(_servo.read() > _target && _const-1 < _target)
  {
    _mPower -= 1;
    _servo.write(_mPower);
    delay(_speed);
  }
 
  if(_servo.read() == _target) ArmReset();
}
 
 
void ArmReset()
{
  _armState = false;
  _inStr = "";
  _inString = "";
  _actionState = 2;
  Serial.println("done");  
}
 
 
String TestAction(int mode) // 복사용 테스트 코드
{
  String _action = "";
  _action = _actions[mode];
 
  if(_testNum >= (sizeof(_actions)/sizeof(_actions[0]))){ // 배열의 수 length
    _testNum = 0;
    _actionState = 0;
    _testState = false;
    Serial.println("Test done"); 
    ServoConnect(false);    
  } else {
    _testNum++;
  }
  
  return _action;
}

 

이전보다 많이? 줄었다.

동작 코드 몇가지를 미리 만들어놔도 될 것 같다.