로봇제작 - 로봇암의 아두이노 코드 완성?

드디어 로봇암을 제어하는 코드를 완성??

최종적인 것은 이전에 만들었던 DC모터 제어 및 블루투스 통신 부분과 합치는 것이겠다.

아래의 코드는 일단 로봇암 제어하는 부분을 일단은 완성했다고 볼 수 있겠다. 다만... 이 부분은 내용을 끝자락에서 다시 설명하겠다.

 

아래의 코드를 간략히 설명하면 작동은 크게 3가지로 구분된다.

첫 번째는 리셋모드로 중간의 어떤 동작에서 바로 초기세팅으로 돌아갈 때 팍 하고 움직이는 것이 아니라 부드럽게 원래 상태로 돌아가는 거라 보면 된다.

두 번째는 입력모드는 시리얼 통신으로 입력된 string 배열을 한 번에 가져와 명령어별로 구분하여 동작하는 방식이다. 실시간 제어도 가능하겠지만 문제는 사물인식과 음성인식 등으로 모바일 환경에서 렉이 걸릴 확률이 높을 것 같아서 선택한 방식이다. 물론 정지 명령어를 보내면 그 즉시 정지된다.

세 번째는 테스트모드로 정해진 동작을 하는 것으로 일반적인 확인 사항이다.

#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;
String _inString = "";
String _inStr = "";
String _inMod = "";
 
bool _armState = false;
bool _servoState = false;
int _mPower = 0;
int _mPowerTarget = 0;
float _mSpeedTime = 0;
 
bool _inputState = true;
int _modeArmState = 0; // 0:대기, 1:입력, 2:테스트, 3:리셋 모드로 정의
 
int _actionNum = 0;
int _actionState = 0; // 0:대기, 1:작동중, 2:작동완료
 
int _count = 0; // 명령수 수 카운트
String _inputActions[20] = {}; // 20개정도 명령어 받아보기
 
String _resetActions[] = {"gx090n10", "gz015n10", "gx035n10", "gc110n10", "gv176n10"}; // 최초값으로 리셋
 
// gt모드로 입력 받을때 자동 실행됨
String _testActions[] = {"gx070n10", "gz080n10", "gx130n10", "gc060n10", "gv140n10", "gz140n10", // 잡기
                         "gv176n10", "gz080n10", "gx090n10", "gz015n10", "gc110n10", "gx035n10" };  // 복귀
 
// 수동입력 테스트 : gx070n90, gz080n90, e
 
void setup()
{
  ServoConnect(false); // 리셋 on / off
 
  Serial.begin(9600);
  Serial.setTimeout(40);
  Serial.println(F("Start~"));
  delay(500);
}
 
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 관절번호 순서
                                     // gt050n10,e > 서보테스트모드, 
                                     // gf050n10,e > 서보off
                                     // gr050n10,e > 서보리셋
    Serial.println(_inString);
 
    _inMod = _inString.substring(0, 1); // 첫째문자 : 모드(이동 or 관절)
    _inStr = _inString.substring(1, 2); // 둘째문자 : 방향 or 관절 > f로 오면 서보off
 
    if(_inMod == "g")
    {
      if(_inStr == "t")       // 테스트모드 시작
        _modeArmState = 2;
      else if(_inStr == "r")  // 리셋모드
      {
        _actionNum = 0;
        _actionState = 0;
        _armState = false;
        
        _modeArmState = 3;
      }        
      else if(_inStr == "f")  // 서보 off
      {
        ServoConnect(false);
        return;
      }
      else // 그밖에 경우에는 입력모드
      {
        _actionNum = 0;
        _actionState = 0;
        _armState = false;
        
        StringSplit(_inString);
        _modeArmState = 1;
      }        
    }
 
    StartAction(_inMod, _inStr); // m, g모드 > gt일 경우 테스트모드로 변경  
  }  
 
  if(_actionState == 2)     // 작동중
  {
    if(_modeArmState == 1)  // 입력모드 반복 실행
    {
      if(_actionNum < _count) // 현재 액션단계가 입력 받은 액션수보다 작을때만 진행
        StartAction(_inMod, _inStr);
    }
    else if(_modeArmState == 3)
      StartAction(_inMod, _inStr);
    else if(_modeArmState == 2) // 테스트모드만 반복 실행
      StartAction("g", "t");
  }
 
  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 StartAction(String mode, String inStr) // 명령어 처리
{
    if(mode == "m")    // 이동 모드
  {
    
  }
  else if(mode == "g") // 로못 암 모드
  {
    if(_modeArmState == 1)  // 입력 모드
      _inString = ReadAction(_actionNum, 1); // 입력 받은 액션 가져오기
    else if(_modeArmState == 3)
      _inString = ReadAction(_actionNum, 0);  // 리셋 액션 가져오기
    else if(_modeArmState == 2 && inStr == "t")
      _inString = ReadAction(_actionNum, 2); // 기존 정해진 테스트액션 가져오기
 
    _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);  // 이동값(시간속도)
  
  if(mode == "m") // 이동 모드
  {
    
  }
  else if(mode == "g") // 관절동작 모드
  {
    if(!_armState)
    {
      if(_modeArmState == 0)
        return;
      
      if(!_servoState)  // 서보가 비활성화일때만 활성화 함
        ServoConnect(true);
 
      _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 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();
 
    _modeArmState = 0;
    _armState = false;
    _inMod = "";
    _inStr = "";
    _inString = "";
    _actionNum = 0;
    _actionState = 0;
  }
 
  _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) 
  {
    _armState = false;
    _inStr = "";
    _inString = "";
    _actionState = 2;
    Serial.println(F("done"));
  }
}
 
String ReadAction(int readNum, int type) // 정해진 액션코드 가져오기
{                                        // type 0:초기화, 1:입력값, 2:테스트
  String _action = "";
  int _length = 0;
 
  switch(type)
  {
    case 0: // 서보모터 초기화 
      _action = _resetActions[readNum];
      _length = sizeof(_resetActions)/sizeof(_resetActions[0]); // 배열의 수 length
       break;
    case 1: // 입력받은 값으로 배열 실행
      _action = _inputActions[readNum];
      _length = sizeof(_inputActions)/sizeof(_inputActions[0]);
      break;
    case 2: // 전체적인 수행 기존 테스트
      _action = _testActions[readNum];
      _length = sizeof(_testActions)/sizeof(_testActions[0]); // 수행 수
    break;
  }
 
  if(_actionNum >= _length)
  {
    _actionNum = 0;
    _actionState = 0;
    _modeArmState = 0;
    ServoConnect(false);
    Serial.println(F("Completed"));
  } 
  else 
  {
    _actionNum++;
  }
 
  
  return _action;
}
 
 
void StringSplit(String inputString)
{
  _count = 0;
  int _getIndex = 0;  
  String _tempString = "";
  String _getString = inputString;  
 
  while(true)
  {
    _getIndex = _getString.indexOf(','); // ','으로 분리된 문자
    
    if(_getIndex != -1) // ','이 있을 경우
    {
      _tempString = _getString.substring(0, _getIndex); // 임시로 ',' 앞부분 문자 가져옴
      _inputActions[_count] = _tempString; // 명령어 배열에 넣기
 
      _getString = _getString.substring(_getIndex + 1); // 앞문자 자르고 다음 전체문자로 저장
      _getString.trim();  // 공백 제거
    } else {
      break; // 없으면 종료
    }
    _count++;
  }
}

 

해당 명령어들이 통신으로 보내면 해당 명령어를 순차적으로 각각 동작한다.

일단 3가지 동작을 만들었는데.. 

사실 다 만들고 보니.. 굳이 3개로 구분할 필요가 없었다.

 

입력동작 하나면 모든 것을 다 할 수 있었던 것이다.. 이걸 완성하고 깨달았다. 어이구.. 바보...

시험 삼아 테스트 명령어들을 다 입력모드로 입력해 보니 당연히 똑같이 움직이고, 리셋 명령어도 입력해 보니 동일한 움직임을 보였다..(물론 끝 부분의 처리 차이는 좀 있다.)

 

이제 입력모드 하나만 있으면 되므로 다시 뜯어고칠까 생각 중이다.,..,..,..,,.

고치게 되면 코드 양은 줄어들 것 같다.

안 그래도 string이라 메모리를 많이 차지하는데...