달력

2

« 2025/2 »

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
2016. 2. 8. 02:58

ESC, IMU, 무선조종기 수신기 각각을 테스트하다 작품/쿼드콥터2016. 2. 8. 02:58

우선 코드를 조금씩 바꿔서 ESC와 IMU, 무선조종기 수신기 각각이 아두이노 MEGA2560에서 제대로 돌아가는 지 보았다.

그 결과 일단 각각은 모두모두 잘 돌아감.


ESC 테스트

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
#include "SoftwareSerial.h"
#include "Servo.h"
 
/////////////////BLDC Motor/////////////////
#define MAX_THROTTLE 180
#define MIN_THROTTLE 0
#define MAX_PWM 150
#define MIN_PWM 0
#define MOTOR1 11  //motor pin number
#define MOTOR2 12  //motor pin number
#define MOTOR3 13  //motor pin number
#define MOTOR4 5 //motor pin number
 
int throttle = 0;
 
Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;
////////////////////////////////////////////
 
 
 
//---------------------------------------------------------------------//
//
//                              funtions
//
//--------------------------------------------------------------------//
 
void callibration(void)
{
  
  Serial.println("Calibrate ESC? 'y' or 'n'.");
    
  while (!Serial.available());
  char cali = Serial.read();
  if(cali == 'y'){  
    Serial.println("send max throttle");
    
    motor1.attach(MOTOR1);
    motor2.attach(MOTOR2);
    motor3.attach(MOTOR3);
    motor4.attach(MOTOR4);
    motor1.write(MAX_THROTTLE);
    motor2.write(MAX_THROTTLE);
    motor3.write(MAX_THROTTLE);
    motor4.write(MAX_THROTTLE);
   
    delay(5000);
 
     Serial.println("send min throttle");
    motor1.attach(MOTOR1);
    motor2.attach(MOTOR2);
    motor3.attach(MOTOR3);
    motor4.attach(MOTOR4);
    motor1.write(MIN_THROTTLE);
    motor2.write(MIN_THROTTLE);
    motor3.write(MIN_THROTTLE);
    motor4.write(MIN_THROTTLE);
 
    delay(2000);
    
    Serial.println("callibration complete");
    
  }else if(cali == 'n'){
    motor1.attach(MOTOR1);
    motor2.attach(MOTOR2);
    motor3.attach(MOTOR3);
    motor4.attach(MOTOR4);
    motor1.write(MIN_THROTTLE);
    motor2.write(MIN_THROTTLE);
    motor3.write(MIN_THROTTLE);
    motor4.write(MIN_THROTTLE);
   
  }
  Serial.println("If you want Program keep going. Press any key.");
  while (!Serial.available());
}
 
void send_val(void)
{
   if(Serial.available()){
    
    char c = Serial.read();
    if(c=='q'){
      throttle += 1;
      Serial.print("throttle = ");Serial.println(throttle);
    }else if(c=='w'){
      throttle -= 1;
      Serial.print("throttle = ");Serial.println(throttle);
    }else if(c=='e'){
      throttle= MAX_PWM;
      Serial.print("throttle = ");Serial.println(throttle);
    }else if(c=='r'){
      throttle= MIN_PWM;
      Serial.print("throttle = ");Serial.println(throttle);
    }
   }
}
 
//제어값을 받어서 모터 구동
void run_motors(int val)
{
  int motor1Power, motor3Power; // Motors on the X axis
  //=>; changing their speeds will affect the pitch
  int motor2Power, motor4Power; // Motors on the Y axis
  //=>; changing their speeds will affect the roll
 
  motor1Power = val;  
  motor2Power = val;  
  motor3Power = val; 
  motor4Power = val; 
  
  motor1.write(motor1Power);motor3.write(motor3Power);
  motor2.write(motor2Power);motor4.write(motor4Power);
  }    
 
 
void setup() {
   Serial.begin(115200); 
 
   callibration();
}
 
void loop() {
  send_val();
  run_motors(throttle);
}
cs


IMU테스트

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
typedef union {
  signed short value;
  unsigned char byte[2];
} S2BYTE;
 
S2BYTE data_word;
 
unsigned char SBuf[100];
int SCnt=0;
double  DegRoll, DegPitch, DegYaw;
 
int recieve_data(void)
{
    static int step_=0, check_all_recieve=0;
    unsigned char data1byte;
    int n;
 
    data1byte = Serial1.read();
    
    switch(step_)
    {
        case 0:
           if(data1byte==0x55) {step_=1, check_all_recieve=0;}
        break;
 
        case 1:
           if(data1byte==0x55) { step_=2;  SCnt=0;}
           else step_=0;
        break;
 
        case 2:
           SBuf[SCnt++] = (unsigned char)data1byte;
           if(SCnt==8)  // roll(2byte),pitch(2byte),yaw(2byte),checksum(2byte) 
             {   
                step_=0;
                SCnt=0;
                check_all_recieve = 1;
             }
        break;
 
        default:
        break;
     }
   
    return check_all_recieve;
}
 
static int trans_data_to_degree(unsigned char *sdata)  // sdata : roll(2)+pitch(2)+yaw(2)+chk(2)
{
  int n;
  unsigned short chk=0;
 
  ///////////// checksum /////////////////
  chk=0x55+0x55;
  for(n=0;n<6;n++) chk+=sdata[n]; 
  if(chk != get_data_word(&sdata[6])) 
  {
    Serial.println("checksum error!");
    return 0; //   
  }
  ////////////////////////////////////////
 
  DegRoll  = get_data_word(&sdata[0]) / 100.;
  DegPitch = get_data_word(&sdata[2]) / 100. * -1;
  DegYaw   = get_data_word(&sdata[4]) / 100.;
 
  return 1;
}
 
static signed short get_data_word(unsigned char *dat)
{
  // little endian..
  data_word.byte[0]=dat[1];  
  data_word.byte[1]=dat[0];  
 
  // big endian..
  //data_word.byte[1]=dat[1];  
  //data_word.byte[0]=dat[0];  
  
  return data_word.value;
}
 
void setup() {
  Serial.begin(115200);
  Serial1.begin(57600);
}
 
void loop() { 
 
  
  if(Serial1.available()){
    if(recieve_data()==1)  //roll(2byte),pitch(2byte),yaw(2byte),checksum(2byte) all recieve!!!
    {   
     if(trans_data_to_degree(SBuf)==1)  
     {
       Serial.print(DegRoll);Serial.print(", ");Serial.print(DegPitch);Serial.print(", ");Serial.println(DegYaw);
     }
    }
  }
 
 
}
cs


무선조종기 수신기 테스트

아래 코드에서 주의 할 점은 attachInterrupt()함수에서 첫번째 인자가 핀 번호가 아니라 인터럽트 번호인것이다.

이것때문에... 많이 고생했지..

Board                     int.0 int.1 int.2 int.3 int.4 int.5

Uno, Ethernet            2      3
Mega2560                 2      3     21     20   19   18
Leonardo                  3      2     0       1      7

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
#define CH1 0
#define CH2 1
#define CH3 2
#define CH4 3
 
volatile int  CH1_prev_time, CH1_pwm_value;
volatile int  CH2_prev_time, CH2_pwm_value;
volatile int  CH3_prev_time, CH3_pwm_value;
volatile int  CH4_prev_time, CH4_pwm_value;
 
void rising1()
{
  attachInterrupt(CH1, falling1, FALLING);
  CH1_prev_time = micros(); 
}
 
void falling1()
{
  attachInterrupt(CH1, rising1, RISING);
  CH1_pwm_value = micros() - CH1_prev_time;
}
 
void rising2()
{
  attachInterrupt(CH2, falling2, FALLING);
  CH2_prev_time = micros(); 
}
 
void falling2()
{
  attachInterrupt(CH2, rising2, RISING);
  CH2_pwm_value = micros() - CH2_prev_time;
}
 
void rising3()
{
  attachInterrupt(CH3, falling3, FALLING);
  CH3_prev_time = micros(); 
}
 
void falling3()
{
  attachInterrupt(CH3, rising3, RISING);
  CH3_pwm_value = micros() - CH3_prev_time;
}
 
void rising4()
{
  attachInterrupt(CH4, falling4, FALLING);
  CH4_prev_time = micros(); 
}
 
void falling4()
{
  attachInterrupt(CH4, rising4, RISING);
  CH4_pwm_value = micros() - CH4_prev_time;
}
 
void setup() {
 Serial.begin(115200);
 attachInterrupt(CH1, rising1, RISING);
 attachInterrupt(CH2, rising2, RISING);
 attachInterrupt(CH3, rising3, RISING);
 attachInterrupt(CH4, rising4, RISING);
}
 
void loop() {
 Serial.print(CH1_pwm_value);Serial.print(", ");
 Serial.print(CH2_pwm_value);Serial.print(", ");
 Serial.print(CH3_pwm_value);Serial.print(", ");
 Serial.print(CH4_pwm_value);Serial.print(", ");
 Serial.println("");
}
 
 
cs



-참고

아두이노 인터럽트

http://www.hardcopyworld.com/ngine/aduino/index.php/archives/594

'작품 > 쿼드콥터' 카테고리의 다른 글

센서값 보정  (0) 2016.02.08
쿼드콥터 하드웨어 완성!  (2) 2016.02.08
보드를 바꾼 후 ESC에 400Hz제어를 시도했지만...  (0) 2016.02.08
아두이노 메가2560으로 MCU를 교체하다  (0) 2016.02.08
무선 조종기와 카메라 테스트  (0) 2016.02.08
:
Posted by youjin.A

티스토리툴바