15. 플로우센서와 라이다센서를 이용한 드론 호버링

 

 

 

• 준비물 
• 라두이노 미니 드론 , SBUS 수신모듈(현재 회로는 R6DSM 사용) Flow Breakout Board(PMW3901+VL53L0X)

 

• 회로도

• 라두이노 미니 코드

#include "raduino_sbus.h"
#include "raduino_fc.h"
#include "PID_v1.h"
#include <Wire.h>                     // i2C 통신을 위한 라이브러리
 
#include "Adafruit_VL53L0X.h"
#include "Bitcraze_PMW3901.h"
 
// Using digital pin 10 for chip select
Bitcraze_PMW3901 flow(PA8); // Optical Motion Tracking Chip
 
double x, y;
 
Adafruit_VL53L0X lox = Adafruit_VL53L0X();// LIDAR Sensor
 
SBUS sbus(Serial2);
FC Flight(Serial1);
 
boolean startFlag = false;
 
unsigned long currentMillis, previousMillis = 0;
 
int ledPin = PC13;
 
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
double Setpoint_1, Input_1, Output_1;
double Setpoint_2, Input_2, Output_2;
int Throttle;
 
double Kp=0.2, Ki=0.3, Kd=0.10;  // 안정 1
 
double Kp_1=32.0, Ki_1=0.3, Kd_1=21.0; 
double Kp_2=32.0, Ki_2=0.3, Kd_2=21.0; 
 
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);                 //고도, THROTTLE
PID myPID_1(&Input_1, &Output_1, &Setpoint_1, Kp_1, Ki_1, Kd_1, DIRECT);   //X-좌표, ROLL
PID myPID_2(&Input_2, &Output_2, &Setpoint_2, Kp_2, Ki_2, Kd_2, DIRECT);   //Y-좌표, PITCH
 
const int numReadings = 5;
int readings[numReadings];
int readIndex = 0;
int total = 0;
int average = 0;
 
 
void setup() {
  Serial.begin(115200);     
//  Serial3.begin(115200);
           
  Serial.println("Adafruit VL53L0X test");
  if (!lox.begin()) {
    Serial.println(F("Failed to boot VL53L0X"));
    while(1);
  }
  
  if (!flow.begin()) {
    Serial.println("Initialization of the flow sensor failed");
    while(1) { }
  }    
 
//--------------------------------------PID Control----------------------------------------------------//
  //Setpoint 설정
  Setpoint = 400;  // 300cm 고도
  Setpoint_1 = 0;
  Setpoint_2 = 0;
 
  //PID OUTPUT 범위 세팅
  myPID.SetOutputLimits(-100, 100);//100,500
  myPID_1.SetOutputLimits(-200, 200);
  myPID_2.SetOutputLimits(-200, 200);
 
  //PID SampleTime 설정
  myPID.SetSampleTime(50);//50ms
  myPID_1.SetSampleTime(50);//50ms
  myPID_2.SetSampleTime(50);//50ms
  
  //turn the PID on
  myPID.SetMode(AUTOMATIC);
  myPID_1.SetMode(AUTOMATIC);
  myPID_2.SetMode(AUTOMATIC);
 
//--------------------------------------PID Control----------------------------------------------------//
  
  sbus.begin();
  sbus.SBUS_OUT= true; //enable SBUS_OUT
 
  // Pause the timer while we're configuring it
  Timer1.pause();  // FC control
 
  // Set up period
  Timer1.setPeriod(10000); // in 10 microseconds
 
  // Set up an interrupt on channel 1
  Timer1.setChannel1Mode(TIMER_OUTPUT_COMPARE);
  Timer1.attachCompare1Interrupt(handler_10ms);
 
  // Refresh the timer's count, prescale, and overflow
  Timer1.refresh();
 
  // Start the timer counting
  Timer1.resume();
 
  Flight.begin();
 
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);
 
  delay(100);
 
  // roll positive right
  // pitch positive forward
  //Flight.Trim(20,10); //roll: 0.0 , pith: -5.0
  Serial.println("Triming...");
 
  delay(5000);
  
  // binding.....
  Flight.Bind();
  Serial.println("binding...");
      
}
 
int ChannelValue[4];
int ch1,ch2,ch3,ch4;
int ch9,ch10;
long long LastTime, PostLastTime, SubLastTime;
int G_COUNT = 0;
int HOLD_COUNT =0;
boolean ledstate =0;
int16_t deltaX,deltaY;
 
VL53L0X_RangingMeasurementData_t measure;   
 
void loop() 
{
 
  int ret = Flight.Event();
  if(ret==1)digitalWrite(LED_BUILTIN,LOW); 
  if(ret>0)Serial.println(Flight.retString);
  
 
  if ( startFlag == true ) {
    ChannelValue[3] += 80;
  }
 
  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;
      }
  }
 
//---------------라이다 센서값 입력---------------------------------------//
  lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
 
  if (measure.RangeStatus != 4) {  // phase failures have incorrect data
//    Input = measure.RangeMilliMeter;
    total = total - readings[readIndex];
    readings[readIndex] = measure.RangeMilliMeter;
    total = total + readings[readIndex];
    readIndex += 1;
      if(readIndex >= numReadings) {
        readIndex = 0;
      }
 
      average = total/numReadings;
      Input = average;
    } 
  else {
    Input = 1000;
  }
//---------------------------------------------------------------------//
 
//---------------X,Y 좌표 변화값 입력----------------------------------//
  // Get motion count since last call
  flow.readMotionCount(&deltaX, &deltaY);
 
  x += (deltaX);
  y += (deltaY);  
  Input_1 = -x/60;
  Input_2 = y/60;
 
}
 
 
void handler_10ms(void) {
  int CHANGE_XY =0;
  
  sbus.process();
 
  ch1  = sbus.getChannel(1); 
  ch2 = sbus.getChannel(2);
  ch3 = sbus.getChannel(3);
  ch4  = sbus.getChannel(4); 
  ch9  = sbus.getChannel(9);  
  ch10  = sbus.getChannel(10);  
  long LostFrames = sbus.getLostFrames();
  LastTime = sbus.getLastTime();
 
  G_COUNT++;
  if(G_COUNT > 100)
  {
    SubLastTime = LastTime - PostLastTime;
    G_COUNT = 0;
    PostLastTime = LastTime;
    
    ledstate = !ledstate;
    if ( startFlag == true ) digitalWrite(ledPin, ledstate);
  }
 
  if (( LostFrames > 25 ) || (SubLastTime < 10)) {
    startFlag = false;
    LostFrames = 0;
  }
  
  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(ch3>1200) Setpoint++;
  //if(ch3<800) Setpoint--;
  int throttle =0;
  
  if((ch9 != 0)&&(startFlag ))//DOWN, 고도 PID ON
  {     
      myPID.Compute(); //고도 PID
       
      throttle= (int)Output+ChannelValue[3];  //고도 , Throttle
      if(throttle <100) throttle = 100;
      if(throttle >900) throttle = 900;     
  }else
  {
      throttle= ChannelValue[3]; // 조종값 바이패스
  }
 
  //x,y좌표 유지시 관성에 의해 흐르는 값을 보정을 위해 조종이 멈춤 후 0.5초후에 좌표 초기화  
  if((abs(ch1-1000)+abs(ch2-1000))<10)
  {
      CHANGE_XY =1;
      HOLD_COUNT--;
      if(HOLD_COUNT <0) HOLD_COUNT = 0;
  }
  else
  {
      CHANGE_XY =0;
      HOLD_COUNT++;
      if(HOLD_COUNT > 50) HOLD_COUNT = 50;
  } 
  
  if((ch10 != 0)&&CHANGE_XY&&(HOLD_COUNT==0))//DOWN, X,Y PID ON
  {      
      myPID_1.Compute(); //X좌표 PID 
      myPID_2.Compute(); //Y좌표 PID
           
      ChannelValue[0]= 512+(int)Output_1; //X좌표 , ROLL
      ChannelValue[1]= 512+(int)Output_2; //Y좌표 , PITCH     
  }else
  {
      x =0;  //x,y 좌표 초기화
      y =0;
  }
 
   if ( startFlag == true ) Flight.Tx(ChannelValue[0],ChannelValue[1],ChannelValue[2],throttle);
}

 

• 소스 코드 다운로드

↓↓↓↓↓↓↓↓↓↓↓↓

 

http://www.raduino.co.kr/

 

↑↑↑↑↑↑↑↑↑↑↑↑

 

자료실에서 다운로드 받을 수 있습니다.