로봇제작 - 아두이노 코드 업데이트 완료

지난번 불필요한 것 이후로 최종적으로 업데이트를 진행했다. 

전체적으로 3가지 모드가 있었는데 과감히 다 버리고 간결한 리셋과 입력 모드만 남겨두었다. 물론 기존의 기본적인 테스트 동작이나 입력동작이나 동일하게 움직인다. 이렇게 하는 게 더 맞는 것 같다.

현재의 코드는 이후에 DC모터 제어부분 쉽게 결합하기 위해 작업한 사항도 있다.

그래서 최종적인 로봇암의 움직이는 코드는 아래와 같다.

#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;  // 최소각(열린 각) 176(닫힌 각)
 
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 = "";
 
// 수동입력 테스트용 : gx070n10, gz080n10, e
 
void setup()
{
  _resetActions = F("gx090n10, gv135n10, gz015n10, gx035n10, gc110n10, gv176n10, gf050n10, e"); // 최초값으로 리셋 
  
  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 관절번호 순서
                                     // 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 == "f")  // 서보 off
      {
        ServoConnect(false);
        return;
      }
      else // 그밖에 경우에는 입력모드
      {
        _actionNum = 0;
        _actionState = 0;
        _armState = false;
 
        if(_inStr == "r") 
          StringSplit(_resetActions);        
        else
          StringSplit(_inString);
          
        _modeArmState = 1;
      }
    }
    
    _inString = _inputActions[0]; // 가장 첫번째 액션만 가져옴
    StartAction(_inMod, _inStr);
  }  
 
  if(_actionState == 2 && _modeArmState == 1)     // 반복 작동중
  {
    if(_inStr == "f")  // 받은 입력액션 중 "f"가 있으면 정지 서보off / 초기화
    {
      ServoConnect(false);
      return;
    }
    else if(_actionNum < _count) // 현재 액션단계가 입력 받은 액션수보다 작을때만 진행
    {
        StartAction(_inMod, _inStr);
    }
  }
 
  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); // 입력 받은 액션 가져오기
  }
 
  _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;
 
  _action = _inputActions[readNum];
  _length = sizeof(_inputActions)/sizeof(_inputActions[0]);
 
  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++;
  }
}

 

이에 따라 메모리도 좀 더 확보되었다. 그래서 이 부분을 좀 캡처해 봤다.

기존 테스트 모드까지 있었던 코드는 아래와 같고 그 다음이 개선 업데이트한 코드이다.

테스트동작 삽입 코드

 

테스트동작 삭제 및 업데이트 코드

 

오~ 저장공간은 1% 정도 이득...

그리고 동적 메모리는 무려 11% 확보... 역시 string은 메모리를 많이 차지한다.

합치면 또 얼마나 커질런지..;;

 

* 2024.04.08 오류가 발생하여 최근 아두이노 스케치 코드를 수정하였습니다.

> 치명적인 오류는 아니고, 속도가 적용이 안된 사항으로 index+1 해야 되는데 index+2가 되어 속도가 전부 0값이 된 사항입니다. 0값이라는 것은 delay가 0라는 것으로 속도가 최대값이라는 것을 의미합니다.