로봇제작 - 로봇암 전체 동작 테스트

아직 배선 정리가 잘 안 되지만 지난번 조립한 것을 전체 동작을 확인해 보기 위해 아두이노 자체적으로 테스트 코드를 작성하였다.

 

결과적으로 동작이 너무 휘청휘청 거린다..;;;

 

이건 뭐 어쩔 수 없겠다. 담엔 좀 더 튼튼한 프레임으로 해봐야겠다. 모두 완성되면 현재의 프레임을 3D나 이미지로 출력한 곳에 측정 수치를 별도로 적어놔야겠다.

어쨌든 프로토타입의 하드웨어 부분은 완성되었다고 보면 될 것 같다. 다만 배선 부분은 첫 번째 프레임 밑에 아크릴을 뚫어서 그쪽으로 배치해야겠다. 자꾸 걸려버린다.;;;

 

아두이노의 테스트 코드는 아래와 같다. 배터리 절약을 위해 로봇암을 움직이지 않을 때는 연결을 해제하고 움직일 때 연결하도록 코드를 구성했다.

모터 동작 소리가 자꾸 거슬려서 그렇게 처리했더니 조용하다. 움직일 때만 동작하는 소리 나고.. 이게 괜찮은 것 같다.

테스트 코드는 최종 블루투스의 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;  // 최소각
 
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 _t = 0;
 
void setup() 
{
  ServoConnect(true); // 리셋
  delay(500);
  ServoConnect(false);
 
  Serial.begin(9600);
  Serial.setTimeout(40);
  Serial.println("Start~");
  delay(500);
}
 
void TestStart()
{
  // test code
  _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+2, _stringLength);  // 이동값(시간속도)
 
  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 loop() 
{
  delay(20);
  
  if(Serial.available() && _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 = true;
      TestStart();
    } else if (_inMod == "f")
      testState = false;
  }
 
  if(testState)
  {
    if(_t > 5000){
      _t = 0;
      TestStart(); 
    }
    else
      _t += 20;
  }
 
  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(90);   // 기본 중간각, 각도가 클수록 위쪽으로 
    servoM_4.write(176);  // 집게 : 135도 ~ 176도 유효각도
  }
  else
  {
    servoM_1.detach();
    servoM_2.detach();
    servoM_3.detach();
    servoM_4.detach();
  }
 
  _servoState = mode;
  delay(500);
}
 
 
void ArmActive(Servo _servo, int _const, int _target, int _speed)
{
  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(_servo);
}
 
 
void ArmReset(Servo _servo)
{
  _armState = false;
  _inStr = "";
  _inString = "";
  Serial.println("done");
}
 
 
String TestAction(int mode){ // 복사용 테스트 코드
 
  String _action = "";
 
  switch(mode)
  {
    case 0 :
      _action = "gx090n50";
      Serial.println("Servo 2");
    break;
    case 1 :
      _action = "gz090n100";
      Serial.println("Servo 1");
    break;
    case 2 :
      _action = "gc100n50";
      Serial.println("Servo 3");
    break;
    case 3 :
      _action = "gv0140n100"; // 그립 열기
      Serial.println("Servo 4");
      break;
    case 4 :
      _action = "gv0176n100"; // 그림 닫기
      Serial.println("Servo r4");
      break;
    case 5 :
      _action = "gc090n100";
      Serial.println("Servo r3");
    break;
    case 6 :
      _action = "gz015n100";
      Serial.println("Servo r1");
    break;
    case 7 :
      _action = "gx035n100";
      Serial.println("Servo r2");
    break;
    default:
      _action = "";
  }
 
  if(testNum < 7)
    testNum++;
  else
  {
    testNum = 0;
    testState = false;
  }
  
  return _action;
}

 

위의 테스트 코드를 적용한 동영상의 결과가 아래와 같다. 각도는 코드 주석에 달아놓은 것처럼 명령어에 따라 원하는 데로 정할 수 있다.  엇.. 배열로 하면 짧아지는데..;;;

오작동 방지를 위해 코드 내에 제한 값을 둬서 짜고 싶었으나 거기까지 생각하기 귀찮아서 그냥 정해진 명령어 대로 입력하면 될 것 같다. 수동모드는 아직 생각하지 않고 있다. 

그냥 사물이 센서에 의해 잡을 수 있는 위치에 있다면 자동으로 잡고 놓을 수 있도록만 하면 될 것 같다.

로봇암 전체 동작 테스트

 

일단 생각보다 잘 움직이는 것 같다.