#include <avr/pgmspace.h>
#include <Servo.h>
#include <Kalman.h>
#include <PID_v1.h>
#define AIL1 8 //AIL fuyi
#define LED 13
unsigned long INAIL;
unsigned long INELE;
int OUTAIL;
int delta = 0;
Servo AIL;
const int TrigPin = 6; //超声波壁障
const int EchoPin = 7;
float distance1;
Servo myservo; // create servo object to control a servo
volatile int state = LOW;
#define IMU_SAMPLE_RATE 1.0/16 // 23.5 Hz
//Kalman kalman_roll(IMU_SAMPLE_RATE, 0.3, 0.003, 0.001);
Kalman kalman_ym(IMU_SAMPLE_RATE, 0.002 , 0.003 , 0.001 );
Kalman kalman_dt(IMU_SAMPLE_RATE, 0.002 , 0.003 , 0.001 );
int pinCSBin = 10;
int pinCSBout = 11;
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
PID myPID(&Input, &Output, &Setpoint, 2 , 0.5 , 1 , DIRECT);
bool pidsw = false;
unsigned long Rx_Ch1_micros;
unsigned long Rx_Ch2_micros;
unsigned int ch0 = 1000;
unsigned int ch6 = 1000 ; //二段开关初始值
void Rx_Ch1(){
if (digitalRead(2)==HIGH) Rx_Ch1_micros=micros(); //Pin 2 油门(接收机)
else ch0=micros()-Rx_Ch1_micros;
}
void Rx_Ch2(){
if (digitalRead(3)==HIGH) Rx_Ch2_micros=micros(); //Pin 3 二段开关
else ch6=micros()-Rx_Ch2_micros;
}
void setup() {
Serial.begin(9600);
pinMode(AIL1,0);
AIL.attach(9);//超声波壁障
pinMode(TrigPin, OUTPUT); //
pinMode(EchoPin, INPUT);
pinMode(pinCSBin, INPUT);
pinMode(pinCSBout, OUTPUT);
attachInterrupt(0, Rx_Ch1, CHANGE); //On Pin 2