13. 적외선 고도제어

• 준비물
  

• 회로도
  

• 소스 코드 다운로드

raduino_irsensoralti.ino

• 코드

#include "raduino_sbus.h"
 
#include "raduino_fc.h"
 
#include "PID_v1.h"
 
#include <Wire.h>                     // i2C 통신을 위한 라이브러리
 
SBUS sbus(Serial2);
 
FC Flight(Serial1);
 
 
 
#define IR_Sensor PA0 // Sharp IR GP2Y0A41SK0F (4-30cm, analog)
 
 
 
//--------------------------------------PID Control----------------------------------------------------//
 
 
 
#define PIN_OUTPUT 3
 
 
 
//Define Variables we'll be connecting to
 
double Setpoint, Input, Output;
 
int Throttle;
 
 
 
//Specify the links and initial tuning parameters
 
double Kp=0.1, Ki=0.3, Kd=0.05;
 
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
 
 
 
//---------------------------------------PID Control---------------------------------------------------//
 
 
 
long cur_goodFrames, old_goodFrames;
 
byte RxValue[11];
 
 
 
int ledPin = PC13;
 
 
 
int Thro, flyFlag, flyState = 0; 
 
boolean startFlag = false;       
 
 
 
unsigned long currentMillis, previousMillis = 0;
 
 
 
int Count = 10;
 
float Distance, SumDistance, AverageDistance = 0;
 
float Voltage = 0; 
 
 
 
// We'll use timer 1
 
HardwareTimer timer(1);
 
 
 
void setup()
 
{
 
    sbus.begin();
 
    sbus.SBUS_OUT= true; //enable SBUS_OUT
 
    
 
    // Pause the timer while we're configuring it
 
    timer.pause();
 
 
 
    // Set up period
 
    timer.setPeriod(10000); // in 10 microseconds
 
 
 
    // Set up an interrupt on channel 1
 
    timer.setChannel1Mode(TIMER_OUTPUT_COMPARE);
 
    timer.setCompare(TIMER_CH1, 1);  // Interrupt 1 count after each update
 
    timer.attachCompare1Interrupt(handler_10ms);
 
 
 
    // Refresh the timer's count, prescale, and overflow
 
    timer.refresh();
 
 
 
    // Start the timer counting
 
    timer.resume();
 
  
 
    Serial.begin(115200);  
 
    Serial3.begin(9600);
 
 
 
    pinMode(LED_BUILTIN, OUTPUT);
 
    digitalWrite(LED_BUILTIN, HIGH);
 
 
 
    delay(100);
 
 
 
    // roll positive right
 
    // pitch positive forward
 
    Flight.Trim(-10,10); //roll: -1.0 , pith: -2.0
 
    Serial.println("Triming...");
 
 
 
    delay(5000);
 
    
 
    // binding.....
 
    Flight.Bind();
 
    Serial.println("binding...");
 
//--------------------------------------PID Control----------------------------------------------------//
 
    Setpoint = 200;  // cm, 셋포인트는 입력값의 단위와 같다. 출력값은 쓰로틀 값으로 함.
 
    //turn the PID on
 
    myPID.SetMode(AUTOMATIC);
 
//--------------------------------------PID Control----------------------------------------------------//
 
}
 
 
 
int ChannelValue[4];
 
  
 
void loop(){
 
  for(int i = 0; i < Count; i++) {  // 5v
 
    Voltage = analogRead(IR_Sensor)*0.0008056640625;  // value from IR_Sensor * (3.3/2^12)
 
    Distance = 13*pow(Voltage, -1); // worked out from datasheet graph
 
    SumDistance = SumDistance + Distance;
 
  }
 
  AverageDistance =SumDistance/Count * 10; 
 
  SumDistance = 0;
 
  if(AverageDistance > 350) AverageDistance = 350;  
 
  //Serial3.println(AverageDistance);
 
      
 
  int ret = Flight.Event();
 
  if(ret==1)digitalWrite(LED_BUILTIN,LOW); 
 
  if(ret>0)Serial.println(Flight.retString);
 
  
 
  int ch10 = sbus.getChannel(10);
 
  //Serial.println(ch10);
 
 
 
//--------------------------------------Shutdown----------------------------------------------------//
 
  while(sbus.getChannel(10) == 2000){
 
    flyFlag=0;
 
    startFlag=0;
 
    RxValue[8] = 0;
 
    RxValue[9] = 0;
 
    for(int i = 0; i < Count; i++) {  // 5v
 
      Voltage = analogRead(IR_Sensor)*0.0008056640625;  // value from IR_Sensor * (3.3/2^12)
 
      Distance = 13*pow(Voltage, -1); // worked out from datasheet graph
 
      SumDistance = SumDistance + Distance;
 
    }
 
    AverageDistance =SumDistance/Count * 10; 
 
    SumDistance = 0;
 
    if(AverageDistance > 350) AverageDistance = 350;  
 
    //Serial3.println(AverageDistance);
 
    Serial.print("| IR_Distance : "); Serial.print(AverageDistance);
 
    Serial.print(", | PID_Output : ");  Serial.println(Output);
 
    
 
    sbus.getChannel(10);
 
    if(sbus.getChannel(10) == 0)  break;
 
  }
 
//--------------------------------------Shutdown----------------------------------------------------//
 
      
 
  if(flyFlag==1){
 
    Input = AverageDistance; 
 
    
 
    myPID.Compute();
 
 
 
    if(Output > 500) Output = 500;                  // Output up   limit
 
    else if(Output < 250) Output = 250;             // Output down limit
 
    
 
    ChannelValue[0]= map(sbus.getChannel(1),300,1700,0,1023); //roll
 
    ChannelValue[1]= map(sbus.getChannel(2),1700,300,0,1023); //pitch
 
    ChannelValue[2]= map(sbus.getChannel(4),300,1700,0,1023); //yaw
 
 
 
    Thro = map(sbus.getChannel(3),300,1700,0,1023);
 
    // PID가 작동하면 Throttle을 중립값(=512)으로 두는 것을 권장함.
 
    if(Thro <= 100) ChannelValue[3]= Thro;
 
    else if(Thro > 100 && Thro <= 950) ChannelValue[3]= Output;      // PID 제어기 출력값을 적용
 
    else ChannelValue[3]= Output+(Thro-950);
 
    
 
    Serial3.print("| IR : ");   Serial3.print(AverageDistance);
 
    Serial3.print(", | PID : ");  Serial3.println(Output);
 
    
 
    RxValue[2] = ChannelValue[0]/256;
 
    RxValue[3] = ChannelValue[0]%256;
 
    RxValue[4] = ChannelValue[1]/256;
 
    RxValue[5] = ChannelValue[1]%256;
 
    RxValue[6] = ChannelValue[2]/256;
 
    RxValue[7] = ChannelValue[2]%256;
 
    RxValue[8] = ChannelValue[3]/256;
 
    RxValue[9] = ChannelValue[3]%256;
 
 
 
    Thro = map(sbus.getChannel(3),300,1700,0,1023);
 
    if(Thro < 100){
 
      flyFlag = 0;  //59cm 이하가 되면 착륙한 것으로 간주
 
      Output = 0;     //PID Output 값 초기화
 
    }
 
  }
 
  else{
 
    Thro = map(sbus.getChannel(3),300,1700,0,1023);
 
    if(Thro > 110 &&  AverageDistance < 80) flyFlag = 1;  //조종기 Throttle mappiing 값 100 이하, 80mm 이하가 되면 이륙한 것으로 간주
 
    
 
    ChannelValue[0]= map(sbus.getChannel(1),300,1700,0,1023); //roll
 
    ChannelValue[1]= map(sbus.getChannel(2),1700,300,0,1023); //pitch
 
    ChannelValue[2]= map(sbus.getChannel(4),300,1700,0,1023); //yaw
 
    ChannelValue[3]= map(sbus.getChannel(3),300,1700,0,1023); //throttle
 
    
 
    if(startFlag == true) ChannelValue[3] += 80;
 
    
 
 
 
    Serial3.print("| IR_Distance : ");   Serial3.print(AverageDistance);
 
    Serial3.print(", | PID_Output : ");  Serial3.println(Output);
 
    //Serial3.print(", | Thro : ");        Serial3.println(Thro);
 
   
 
    RxValue[2] = ChannelValue[0]/256;
 
    RxValue[3] = ChannelValue[0]%256;
 
    RxValue[4] = ChannelValue[1]/256;
 
    RxValue[5] = ChannelValue[1]%256;
 
    RxValue[6] = ChannelValue[2]/256;
 
    RxValue[7] = ChannelValue[2]%256;    
 
    RxValue[8] = ChannelValue[3]/256;
 
    RxValue[9] = ChannelValue[3]%256;
 
    
 
    if( ChannelValue[0] < 30 && ChannelValue[1] < 30 && ChannelValue[2] > 1000 && ChannelValue[3] < 110 ){   // 시동 걸때
 
      currentMillis = millis();
 
      if (currentMillis - previousMillis >= 1500){
 
        previousMillis = currentMillis;
 
        startFlag = 1;
 
      }
 
    }
 
 
 
    if( ChannelValue[0] > 1000 && ChannelValue[1] < 30 && ChannelValue[2] < 30 && ChannelValue[3] < 110 ){   // 시동 끌때
 
      currentMillis = millis();
 
      if (currentMillis - previousMillis >= 1500){
 
        previousMillis = currentMillis;
 
        startFlag = 0;
 
      }
 
    }
 
  }
 
 
 
//  Serial.print(ChannelValue[0]);  //312 ~1690 우스틱 좌우 roll
 
//  Serial.print("   |   ");
 
//  Serial.print(ChannelValue[1]); //325 ~1673 좌스틱 좌우 pitch
 
//  Serial.print("   |   ");
 
//  Serial.print(ChannelValue[2]); //352 ~1698 좌스틱 상하 yaw
 
//  Serial.print("   |   ");
 
//  Serial.print(currentMillis); 
 
//  Serial.print("   |   ");
 
//  Serial.println(startFlag);
 
}
 
 
 
void handler_10ms(void) {
 
  sbus.process();
 
 
 
  int ch1 = sbus.getChannel(1); 
 
  int ch2 = sbus.getChannel(2);
 
  int ch3 = sbus.getChannel(3);
 
  int ch4 = sbus.getChannel(4);  
 
  
 
  ChannelValue[0]= map(ch1,300,1700,0,1023); //roll
 
  ChannelValue[1]= map(ch2,1700,300,0,1023); //pitch
 
  ChannelValue[2]= map(ch4,300,1700,0,1023); //yaw
 
  ChannelValue[3]= map(ch3,300,1700,0,1023); //throttle
 
 
 
  if ( flyFlag==1 || startFlag==1 ) Flight.Tx(ChannelValue[0],ChannelValue[1],ChannelValue[2],ChannelValue[3]);
 
} 
 
 
 
출처: http://raduino.tistory.com/31?category=749486 [라두이노, 드론에 상상력을 싣다!]