아두이노 프로젝트 | 마이크로 서보 로봇

1. 마이크로 서보 로봇

마이크로 서보 로봇

   제조 공장에서 흔하게 볼 수 있는 로봇과, 로봇을 제어하기 위한 모형을 아두이노 플랫폼으로 구현해보자. 구현하고자 하는 로봇은 서보모터 4개로, 로봇을 제어하기 위한 모형은 가변저항 4개로 구성되어 있다. 모형을 움직이면 로봇이 움직임을 따라가도록 프로그램을 작성해보자. 추가로, 모형의 움직임을 차례대로 저장한 뒤 저장된 움직임을 로봇이 반복하는 기능을 구현해보자. 모형의 움직임을 저장하고 재생하는 신호는 택트 스위치 1개로 제어하며, 택트 스위치를 한 번 누르면 모형의 움직임이 저장되고, 택트 스위치를 빠르게 두 번 누르면 로봇이 아두이노에 전원이 공급되지 않을 때까지 저장된 움직임을 반복하도록 프로그램을 작성해보자.

 

2. 프로젝트

2.1. 회로 구성

 

회로 구성

아두이노 가변 저항 10KΩ (다이얼 타입)

브레드보드용 4핀 택트스위치 6x6 breadboard friendly switch 4p [SZH-TH0018]

1/2W 1% Axial Resistor 221F (220Ω)

TowerPro 호환 9g 미니 서보모터 SG-90

 

2.2. 프로그램 작성

#include <Servo.h>

Servo myServo1; // myServo1라는 이름의 서보모터 개체를 생성
Servo myServo2;
Servo myServo3;
Servo myServo4;

int Angle[4] = {90, 90, 90, 90}; // 정수형 배열 선언 후 초기 각도 90도로 초기화
int Memory1[100]; // myServo1의 각도 정보를 100개까지 저장할 수 있는 정수형 배열 선언
int Memory2[100];
int Memory3[100];
int Memory4[100];
int IndexCounter = 0; // 저장된 정보 수를 저장할 정수형 변수 선언 후 0으로 초기화
float InputAngle[4]; // 서보모터에 입력할 각도 정보를 저장할 실수형 배열 선언
int Resolution = 10; // 정수형 변수 선언 후 10으로 초기화

int Potentiometer1  = A3; // 정수형 변수 선언 후 아날로그 핀 번호 A0로 초기화
int Potentiometer2  = A2;
int Potentiometer3  = A1;
int Potentiometer4  = A0;

int Switch = 7; // 정수형 변수 선언 후 디지털 핀 번호 7로 초기화
bool State = false; // 택트 스위치의 상태 정보를 저장할 불린 변수 선언 후 false로 초기화
// 택트 스위치가 연속으로 눌린 횟수 정보를 저장할 정수형 변수 선언 후 0으로 초기화
int PushCount = 0;
// 택트 스위치가 마지막으로 눌린 시간 정보를 저장할 정수형 변수 선언 후 0으로 초기화
long LastChange = 0;

void setup() 
{
  myServo1.attach(9); // myServo1을 12번 핀에 연결
  myServo1.write(Angle[0]); // myServo1의 각도를 90도로 초기화
  myServo2.attach(10); 
  myServo2.write(Angle[1]); 
  myServo3.attach(11);
  myServo3.write(Angle[2]); 
  myServo4.attach(12);
  myServo4.write(Angle[3]);
  delay(1000); // 1000 ms 대기

  pinMode(Switch, INPUT); // 디지털 7번 핀을 출력(INPUT) 모드로 설정
  
  pinMode(Potentiometer1, INPUT); // 아날로그 A3번 핀을 출력(INPUT) 모드로 설정
  pinMode(Potentiometer2, INPUT); 
  pinMode(Potentiometer3, INPUT); 
  pinMode(Potentiometer4, INPUT);
 
  Serial.begin(9600); // 시리얼 통신 시작 (통신 속도: 9600 bps)
  Serial.println("*** Recording ***"); // 시리얼 모니터에 문자열을 출력한 뒤 줄 바꿈
}

void loop() {
  // 아날로그 핀으로부터 받은 아날로그 신호값의 범위를 0~1023에서 0~255로 조정한 뒤 
  // 정수형 배열에 저장
  Angle[0] = map(analogRead(Potentiometer1), 0, 1023, 0, 180);
  Angle[1] = map(analogRead(Potentiometer2), 0, 1023, 0, 180);
  Angle[2] = map(analogRead(Potentiometer3), 0, 1023, 0, 180);
  Angle[3] = map(analogRead(Potentiometer4), 0, 1023, 0, 180);

  // 정수형 배열에 저장된 각도 정보만큼 서보모터를 회전
  myServo1.write(Angle[0]);
  myServo2.write(Angle[1]);
  myServo3.write(Angle[2]);
  myServo4.write(Angle[3]);
  delay(10);
     
  // 택트 스위치가 연속으로 눌린 횟수에 따라 프로그램 실행
  switch (MultiPressRead(Switch)) 
  {  
    // 택트 스위치가 한 번만 눌린 경우
    case 1:
      // 모든 서보모터의 현재 각도 정보를 메모리 배열에 각각 저장
      Memory1[IndexCounter] = myServo1.read(); 
      Memory2[IndexCounter] = myServo2.read();
      Memory3[IndexCounter] = myServo3.read();
      Memory4[IndexCounter] = myServo4.read();
      
      // 메모리 배열에 저장된 각도 정보를 시리얼 모니터에 출력    
      Serial.print("[ ");    
      Serial.print(Memory1[IndexCounter]);
      Serial.print(" | ");
      Serial.print(Memory2[IndexCounter]);
      Serial.print(" | ");
      Serial.print(Memory3[IndexCounter]);
      Serial.print(" | ");
      Serial.print(Memory4[IndexCounter]);
      Serial.println(" ]");
      
      // 메모리 배열에 저장된 정보 수를 저장    
      IndexCounter += 1;
      delay(100);
      break; // switch문 종료
    
    
    
    // 택트 스위치가 두 번 연속으로 눌린 경우
    case 2:
      Serial.println("*** Repetition Start ***");
      delay(1000);
      
      // 전원이 공급되지 않을 때까지 무한 반복    
      while(1) 
      {
        // 모든 서보모터의 현재 각도 정보를 실수형 배열에 저장
        InputAngle[0] = Angle[0];
        InputAngle[1] = Angle[1];
        InputAngle[2] = Angle[2];
        InputAngle[3] = Angle[3];
        
        // 현재 각도에서 다음 각도까지 회전      
      	for (int i = 1; i <= Resolution; i++)
      	{
          // 현재 각도에서 다음 각도까지 최소 회전 단위로 회전하도록 최소 회전 단위 연산
          InputAngle[0] += (Memory1[0] - Angle[0]) / Resolution; 
          InputAngle[1] += (Memory2[0] - Angle[1]) / Resolution;
          InputAngle[2] += (Memory3[0] - Angle[2]) / Resolution;
          InputAngle[3] += (Memory4[0] - Angle[3]) / Resolution;
          // 연산한 최소 회전 단위를 정수형으로 변경한 뒤 모든 서보모터에 입력
          myServo1.write((int)InputAngle[0]); 
          myServo2.write((int)InputAngle[1]);
          myServo3.write((int)InputAngle[2]);
          myServo4.write((int)InputAngle[3]);
          delay(10);
        } 
        
        // 연산에 의한 오차를 보정하기 위해 모든 서보모터에 최종 회전 각도 입력
        myServo1.write(Memory1[0]);
        myServo2.write(Memory2[0]);
        myServo3.write(Memory3[0]);
        myServo4.write(Memory4[0]);
        
        // 현재 서보모터의 각도 정보를 시리얼 모니터에 출력      
        Serial.print("[ ");    
        Serial.print(myServo1.read());
        Serial.print(" | ");
        Serial.print(myServo2.read());
        Serial.print(" | ");
        Serial.print(myServo3.read());
        Serial.print(" | ");
        Serial.print(myServo4.read());
        Serial.println(" ]");
        delay(1000);
        
        // 메모리 배열에 저장된 정보에 맞게 차례대로 동작    
        for (int i = 1; i < IndexCounter; i++)
        {
          // 모든 서보모터의 현재 각도 정보를 실수형 배열에 저장
          InputAngle[0] = Memory1[i - 1];
          InputAngle[1] = Memory2[i - 1];
          InputAngle[2] = Memory3[i - 1];
          InputAngle[3] = Memory4[i - 1];  

          for (int j = 1; j <= Resolution; j++)
          {          
            InputAngle[0] += (Memory1[i] - Memory1[i - 1]) / Resolution;
            InputAngle[1] += (Memory2[i] - Memory2[i - 1]) / Resolution;
            InputAngle[2] += (Memory3[i] - Memory3[i - 1]) / Resolution;
            InputAngle[3] += (Memory4[i] - Memory4[i - 1]) / Resolution;
            myServo1.write((int)InputAngle[0]);
            myServo2.write((int)InputAngle[1]);
            myServo3.write((int)InputAngle[2]);
            myServo4.write((int)InputAngle[3]);
            delay(10);
          }        

          myServo1.write(Memory1[i]);
          myServo2.write(Memory2[i]);
          myServo3.write(Memory3[i]);
          myServo4.write(Memory4[i]);
          Serial.print("[ ");    
          Serial.print(myServo1.read());
          Serial.print(" | ");
          Serial.print(myServo2.read());
          Serial.print(" | ");
          Serial.print(myServo3.read());
          Serial.print(" | ");
          Serial.print(myServo4.read());
          Serial.println(" ]");
          delay(1000);
        }  
      }
      break;
  }
}


// 택트 스위치 연속 동작 판별 함수 
int MultiPressRead(int Switch) 
{    
  if (digitalRead(Switch) == HIGH) // 택트 스위치가 눌린 경우 실행
  {
    if (State == false) // 택트 스위치의 이전 상태가 눌리지 않은 상태일 경우 실행 
    {
      PushCount += 1; // 택트 스위치가 눌린 횟수를 정보를 정수형 변수에 저장
      State = true; // 택트 스위치가 눌린 상태를 불린 변수에 저장
      LastChange = millis(); // 현재 시간 정보를 정수형 변수에 저장
      return 0; // 함수 실행 결과로 0을 반환하고 함수 종료
    }    
  }  
  if (digitalRead(Switch) == LOW) // 택트 스위치가 눌리지 않은 경우 실행
  {    
    State = false; // 택트 스위치가 눌리지 않은 상태를 불린 변수에 저장    
    // 택트 스위치가 연속으로 눌렸는지 판별
    // 택트 스위치가 마지막으로 눌린 시간과 현재 눌린 시간의 차이가 500 ms 이상이면 실행
    if ((millis() - LastChange) > 500) 
    {
      int NumberOfPresses = PushCount;
      PushCount = 0; // 택스 스위치가 눌린 횟수 정보를 초기화
      // 함수 실행 결과로 정수형 변수에 저장된 값을 반환하고 함수 종료
      return NumberOfPresses;
    } 
  }
}

 

 

 

 

 

[함께 읽으면 좋은 페이지]

 

아두이노 디지털 신호 입력 | 택트 스위치

아두이노 | 가변저항

아두이노 라이브러리 | 서보모터

 

 

 

 

 

참고문헌

- Kuantronic. (2021). JC_Button. GitHub. https://github.com/Kuantronic/JC_Button. 2023.10.28.

- Pinaut. (2014). Micro Servo Robot. YouTube. https://www.youtube.com/watch?v=bLnAJ-mSElE. 2023.10.26.