MeArm - 원격 제어 1

MeArm - 원격 제어 1

출처 : 실습4. 아두이노 (Arduino)로 가변저항의 저항 값 읽기 : 네이버 블로그 | 메이키스트

MeArm을 원격 제어 하기 위해서 만들기 시작했는데, 아직 하나의 아두이노에서 제어만 가능한 상태입니다.

Raspberry PI를 이용해서 제어 명령을 녹화 했다가 다시 재생하고, 블루투스로 원역으로 제어 하는 부분을 구현 예정인데, 완료되면 다시 글 올리도록 하겠습니다.

최초 아두이노 소스

#include<Servo.h> 

Servo myservo1; 
Servo myservo2;
Servo myservo3; 
int servoPin1 = 10; 
int servoPin2 = 3; 
int servoPin3 = 5; 
int pos = 0; 

int analogPin1 = A1;     // potentiometer wiper (middle terminal) connected to analog pin 3
int analogPin2 = A2;
int analogPin3 = A3; // outside leads to ground and +5V
int val1 = 0;           // variable to store the value read
int val1_offset_p = 10;
int val1_p_max = 180;
int val1_offset_s = 0;
int val1_ratio = 5.6266;
long temp1_old = 0;
int val2 = 0;           // variable to store the value read
int val2_offset_p = 90;
int val2_p_max = 170;
int val2_offset_s = 60;
int val2_ratio = 6.3636;
long temp2_old = 0;
int val3 = 0;           // variable to store the value read
int val3_offset_p = 100;
int val3_p_max = 180;
int val3_offset_s = 30;
int val3_ratio = 6;
long temp3_old = 0;

void setup()
{
  Serial.begin(9600);          //  setup serial
  myservo1.attach(servoPin1); 
  myservo2.attach(servoPin2); 
  myservo3.attach(servoPin3); 
}

void loop()
{
  val1 = analogRead(analogPin1);    // read the input pin
  val2 = analogRead(analogPin2);    // read the input pin
  val3 = analogRead(analogPin3);    // read the input pin
  Serial.print(val1);             // debug value
  Serial.print(", ");             // debug value
  Serial.print(val2);             // debug value
  Serial.print(", ");             // debug value
  Serial.print(val3);             // debug value
  

  // 오른쪽 0 / 왼쪽 180
  // 오른쪽 1023 / 왼쪽 10
  // 1023-10 / 180 = 5.6266
  long temp1 = val1_p_max + val1_offset_s - ((val1 - val1_offset_p) / val1_ratio);
  if (temp1 < val1_offset_s) {
    temp1 = val1_offset_s;
  }
  if (temp1 > val1_p_max) {
    temp1 = val1_p_max;
  }
  Serial.print(", ");             // debug value
  Serial.print(temp1_old);             // debug value

  // 뒤 170 / 앞 : 60
  // 뒤 90 / 왼쪽 790
  // 790-90 / 170-60 = 6.3636  
  long temp2 = val2_p_max - ((val2 - val2_offset_p) / val2_ratio);
  if (temp2 < val2_offset_s) {
    temp2 = val2_offset_s;
  }
  if (temp2 > val2_p_max) {
    temp2 = val2_p_max;
  }

  
  Serial.print(", ");             // debug value
  Serial.print(temp2_old);             // debug value

  
  
  // 뒤 30 / 앞 : 180
  // 뒤 1000 / 앞 100
  // 1000-100 / 180-30 = 6 
  long temp3 = val3_p_max - ((val3 - val3_offset_p) / val3_ratio);
  if (temp3 < val3_offset_s) {
    temp3 = val3_offset_s;
  }
  if (temp3 > val3_p_max) {
    temp3 = val3_p_max;
  }

  Serial.print(", ");             // debug value
  Serial.print(temp3_old);             // debug value

  if (temp1_old - 25 < temp1  &&  temp1_old + 25 ) {
    myservo1.write(temp1); 
    temp1_old = temp1;  
  }

  if (temp2_old - 25 < temp2  &&  temp2_old + 25 ) {
    myservo2.write(temp2); 
    temp2_old = temp2;  
  }

  if (temp3_old - 25 < temp3  &&  temp3_old + 25 ) {
    myservo3.write(temp3); 
    temp3_old = temp3;  
  }
  
  Serial.print("\n");             // debug value
  delay(25);
}

개선된 소스 - 첫 번째

#include<Servo.h> 

Servo myservo[3];  
int servoPin[] = { 10, 3, 5 }; 

#define ARRAY_LENGTH 3
#define val1_offset_s 0
#define val1_p_max 1
#define val1_offset_p 2
#define val1_ratio 3

int analogPin[] = { A1, A2, A3 }; // outside leads to ground and +5V
double val1_offset[3][4] = {
  { 10, 180, 0, 5.6266 }, 
  { 90, 170, 60, 6.3636},
  { 100, 180, 30, 6} 
};
int val[3];           // variable to store the value read
double temp[] = { 0, 0, 0 };
double temp_old[] = { 0, 0, 0 };
int chcek_int = 3;

void setup() {
  Serial.begin(9600);          //  setup serial
  for (int i=0; i<ARRAY_LENGTH; i++) {
    myservo[i].attach(servoPin[i]);
  }
  //myservo[0].attach(servoPin[0]);
}

void loop() {
  for (int i=0; i<ARRAY_LENGTH; i++) {
    val[i] = analogRead(analogPin[i]);    // read the input pin
    Serial.print(i);
    Serial.print("=");
    Serial.print(val[i]);             // debug value
    Serial.print(",");             // debug value
  }
  Serial.print("|");             // debug value

  // 오른쪽 0 / 왼쪽 180
  // 오른쪽 1023 / 왼쪽 10
  // 1023-10 / 180 = 5.6266
  temp[0] = val1_offset[0][val1_p_max] + val1_offset[0][val1_offset_s] - ((val[0] - val1_offset[0][val1_offset_p]) / val1_offset[0][val1_ratio]);
  
  // 뒤 170 / 앞 : 60
  // 뒤 90 / 왼쪽 790
  // 790-90 / 170-60 = 6.3636 
  temp[1] = val1_offset[1][val1_p_max] - ((val[1] - val1_offset[1][val1_offset_p]) / val1_offset[1][val1_ratio]);
   
  // 뒤 30 / 앞 : 180
  // 뒤 1000 / 앞 100
  // 1000-100 / 180-30 = 6
  temp[2] = val1_offset[2][val1_p_max] - ((val[2] - val1_offset[2][val1_offset_p]) / val1_offset[2][val1_ratio]);

  for (int i=0; i<ARRAY_LENGTH; i++) {
    if (temp[i] < val1_offset[i][val1_offset_s]) {
      temp[i] = val1_offset[i][val1_offset_s];
    }
    if (temp[i] > val1_offset[i][val1_p_max]) {
      temp[i] = val1_offset[i][val1_p_max];
    }
    
    Serial.print(temp_old[i]);             // debug value
    Serial.print("/ ");             // debug value
  }
  
  for (int i=0; i<ARRAY_LENGTH; i++) {
    if (temp_old[i] - chcek_int > temp[i] || temp[i] > temp_old[i] + chcek_int ) {
      myservo[i].write(temp[i]); 
      temp_old[i] = temp[i];  
      Serial.print(", ");             // debug value
      Serial.print("servo");
      Serial.print(i);
      Serial.print(" set");             // debug value
    }
  }
  
  Serial.print("\n");             // debug value
  delay(25);
}

개선된 소스 - 두 번째

#include<Servo.h> 

Servo myservo[3];  
int servoPin[] = { 10, 3, 5 }; 

#define ARRAY_LENGTH 3
#define resi_s 0
#define resi_e 1
#define servo_s 2
#define servo_e 3
#define arm_ratio 4

int analogPin[] = { A1, A2, A3 }; // outside leads to ground and +5V
double cvalue[3][5] = {
  {  50, 1023,   0, 180, 5.6266 }, 
  {  90,  790,  60, 170, 6.3636 },
  { 100, 1000,  30, 180, 6 } 
};
int val[3];           // variable to store the value read
double temp[] = { 0, 0, 0 };
double temp_old[] = { 0, 0, 0 };
int chcek_int = 3;

void setup() {
  Serial.begin(9600);          //  setup serial
  for (int i=0; i<ARRAY_LENGTH; i++) {
    myservo[i].attach(servoPin[i]);

    // 790-90 / 170-60 = 6.3636 
    cvalue[i][arm_ratio] = (cvalue[i][resi_e] - cvalue[i][resi_s])
      / (cvalue[i][servo_e] - cvalue[i][servo_s]); 
  }
}

void loop() {
  for (int i=0; i<ARRAY_LENGTH; i++) {
    val[i] = analogRead(analogPin[i]);    // read the input pin
    Serial.print(i);
    Serial.print("=");
    Serial.print(val[i]);             // debug value
    Serial.print(",");             // debug value
  }
  Serial.print("|");             // debug value

  // 오른쪽 0 / 왼쪽 180
  // 오른쪽 1023 / 왼쪽 10
  // 1023-10 / 180 = 5.6266
  temp[0] = cvalue[0][servo_e] + cvalue[0][servo_s] - ((val[0] - cvalue[0][resi_s]) / cvalue[0][arm_ratio]);
  
  // 뒤 170 / 앞 : 60
  // 뒤 90 / 왼쪽 790
  // 790-90 / 170-60 = 6.3636 
  temp[1] = cvalue[1][servo_e] - ((val[1] - cvalue[1][resi_s]) / cvalue[1][arm_ratio]);
   
  // 뒤 30 / 앞 : 180
  // 뒤 1000 / 앞 100
  // 1000-100 / 180-30 = 6
  temp[2] = cvalue[2][servo_e] - ((val[2] - cvalue[2][servo_s]) / cvalue[2][arm_ratio]);

  for (int i=0; i<ARRAY_LENGTH; i++) {
    if (temp[i] < cvalue[i][servo_s]) {
      temp[i] = cvalue[i][servo_s];
    }
    if (temp[i] > cvalue[i][servo_e]) {
      temp[i] = cvalue[i][servo_e];
    }
    
    Serial.print(temp_old[i]);             // debug value
    Serial.print("/ ");             // debug value
  }
  
  for (int i=0; i<ARRAY_LENGTH; i++) {
    if (temp_old[i] - chcek_int > temp[i] || temp[i] > temp_old[i] + chcek_int ) {
      myservo[i].write(temp[i]); 
      temp_old[i] = temp[i];  
    }
  }
  
  Serial.print("\n");             // debug value
  delay(25);
}

실행