File content as of revision 0:c7c4be6a082f:

#include "mbed.h"
#include "TextLCD.h"
//#include "I2cLCD.h"

DigitalOut myled1(LED1),myled2(LED2),myled3(LED3),myled4(LED4);
AnalogIn pitot(p15);
Serial pc(USBTX,USBRX); //(USBTX,USBRX)
Timer t1,t2;
InterruptIn ch1(p16),ch2(p17);
PwmOut yawyaw(p21),ch5out(p22);

double t1_pwm;
double t2_pwm;

/////////////timer//////////////
void t1_start(){
  t1.start();
}

void t1_stop (){
  t1.stop();
   t1_pwm  =   t1.read();
   t1.reset();
}

void t2_start(){
  t2.start();
}

void t2_stop (){
  t2.stop();
   t2_pwm  =   t2.read();
   t2.reset();
}

///////////////////////////////////




int main(void){
    
    pc.baud(115200);

    double yaw_gain_p = 0.001;
    double yaw_s;
    
    ch1.rise(&t1_start) ;
    ch1.fall(&t1_stop) ;
     
    ch2.rise(&t2_start) ;
    ch2.fall(&t2_stop) ;
    
         
    yawyaw.period(0.020);
    
    double pit_read = pitot.read();
    
   
        while(1){

        
            if (t2_pwm > 0.0015){

                double yaw_error = (pitot.read() - 0.4785);
        
                yaw_s = 0.00148 + yaw_gain_p * yaw_error;
                
                     if (yaw_s > 0.00175){yaw_s = 0.00175;}
                     if (yaw_s < 0.0012){yaw_s = 0.0012;}
                         
        
                //if (t2_pwm > 0.0015){
                yawyaw.pulsewidth(yaw_s); // servo position determined by a pulsewidth between 1-2ms
                
                }
    
        
            else{
                yawyaw.pulsewidth(t1_pwm);}
        
                
                 //lcd.locate(8,1);
                //rot_read = (rotary1.read())*1000000;
                pc.printf("pitot = %f\r\n", pitot.read());
                pc.printf("yaw_s = %f\r\n", yaw_s);
                pc.printf("t1_pwm = %f\r\n", t1_pwm);
                pc.printf("t2_pwm = %f\r\n", t2_pwm);
                //wait(1);
                ch5out.pulsewidth(t2_pwm);
        }
    
}