File content as of revision 1:57378b3160b9:

#include "mbed.h"
#include "rtos.h" //for threads
#include "LSM9DS1.h"
#include "Servo.h"
#include "clock.h"

#define PI 3.14159
#define DECLINATION -2.44
#define deltat_ 0.004f //250Hz
#define deltat_disp 0.164f //~6Hz

//Setup Patameters
Servo servo_1(p25); //left servo
Servo servo_2(p24); //right servo
DigitalOut myled(LED1);
Serial pc(USBTX, USBRX);

AnalogIn ain(A0);
AnalogIn ain1(A1);
LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);//SDA, SDL
//ports for switching circuit
Thread thread;

    float roll;
    float pitch;
    float yaw;
    float left_servo;
    float right_servo;
    //int i;



void Setup()
{
    set_tick_(deltat_);
    set_tick_disp(deltat_disp);   
}

int main()
{
    Setup();
    //Display Setup
        Timer timer;
        Timer timer2;

     //   int timee =0;
    int i=50;
    int j =0;
   // int begin, end;
    // PID config Roll
    float error=0.0;
    float P =0.0;   //Proportional term 
    float I =0.0;   //Intergral term
    float prevD =0.0;
    //float prevDy=0.0l;
    float D =0.0;   //Derivative term
    float k1 = -0.9776;      //Proportaional PID weight
    float k2 = -0.5656;      //Integral PID
    float k3 = -4.2247;      //Derivative PID
    float PV=0.0;           //Process variable
    float SP=0;         //Set Point (Desired Roll in degerees)
    // PID config Pitch
    float errorp =0.0;   //Proportional term 
    float Pp =0.0;   //Proportional term 
    float Ip =0.0; //Intergral term
    float prevDp =0.0;
    float Dp =0.0;   //Derivative term
    float k1p = -1;        //PID weights
    float k2p = 0.5;
    float k3p = 10;
    float PVp=0.0;       //Process variable
    float SPp=0;       //Set Point    
    //PID combine setup 
    float rollc;
    float pitchc;
    float top;
    float bot;
    float centre;
    float diff;
    //Servo initial conditions
    float range = 0.0005;
    float roll_correct = 0;
    float pitch_correct= 0; //Range 0-1
    servo_1.calibrate(range, 45.0);  
    servo_2.calibrate(range, 45.0);
    //PWM conversion
    float left_scale = 0.01778; //convert degrees to PWM. Assumes linear realtionship (is not). optimised for linearity around neutral.
    float right_scale = -0.01778;
    float left_neutral = 0.39; //neutral position PWM
    float right_neutral = 0.61;  
    //IMU
    LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);//SDA, SDL
    IMU.begin();
    if (!IMU.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    IMU.calibrate(0); //If this is set to 1 the Glider will assume it starts level
    double mx;
    printf("Start");
    void display_thread();
    thread.start(display_thread);
    while(1) 
    {
        if(tock())
        {   
        myled=!myled;
            //Attitude     
            while(!IMU.accelAvailable());
            IMU.readAccel();
            roll = atan2((double)IMU.ax, (double)IMU.az); //swtiched the roll and pitch eqautions round
            pitch = atan2(-(double)IMU.ay, sqrt((double)IMU.ax * (double)IMU.ax + (double)IMU.az * (double)IMU.az));
            pitch *= 180.0 / PI;
            roll  *= 180.0 / PI;


            mx = -IMU.mx;
            float heading;
            if (IMU.my == 0.0)
                heading = (mx < 0.0) ? 180.0 : 0.0;
            else
                heading = atan2(mx, IMU.my)*360.0/(2.0*PI);

            
             //If moving up or down adjust the SPp for pitch. Tether should deal with this
            //shift_SPp=IMU.az;//scale with time and shift by 9.81. 
            //SPp+=shift_SPp ; Adjusting the setpoint would require new PID values.
            
            //PID for Roll
            PV =roll; 
            error= PV-SP; //in degrees           
            P = k1*error;
            I += k2*error;
            D = k3*error - prevD; //change in glider's roll in degrees since last sample
            //float N=5.3;
            //D = (k3*error -prevD)*N -(N-1)*prevDy;
            prevD =k3*error;
            //prevDy = D;
            roll_correct = P + I + D ; //Try PID
            //Clamp if PID has saturated integral.
            if(roll_correct<-18 || roll_correct>48) { //Is the PID saturated?
                if(!((k2*error>0)^(roll_correct>0)))  //Is the new intergral contributing to saturation?   
                {
                    I -=k2*error; //Don let accError increase, its saturated
                    roll_correct = P + I + D ; //Update PID
                }
            }
            
            //PID for pitch 
            PVp =pitch; 
            errorp= PVp-SPp; //in degrees           
            Pp = k1p*errorp;
            Ip += k2p*errorp;
            Dp = k3p*errorp - prevDp; //change in glider's roll in degrees since last sample
            prevDp =k3p*errorp;
            pitch_correct = Pp + Ip + Dp ; //Try PID
            //Clamp if PID has saturated integral.
            if(pitch_correct<-18 || pitch_correct>48) { //Is the PID saturated?
                if(!((k2p*errorp>0)^(pitch_correct>0)))  //Is the new intergral contributing to saturation?   
                {
                    Ip -=k2p*errorp; //Don let accError increase, its saturated
                    pitch_correct = Pp + Ip + Dp ; //Update PID
                }
            }
            
            //Update Servos, left and right
            //left_servo = pitch_correct + roll_correct; //Angle deg from -18 to (18)
            //right_servo = pitch_correct +(1 - roll_correct); //Angle deg from -18 to (18)
            
            //Take in the corrects and combine them for Left and Right elevator
                rollc = (roll_correct/2);
                pitchc = (-pitch_correct-15);
                top = rollc+15;
                bot = 15-rollc;
                centre = 15;
                
                while (pitchc != 0)
                {
                    if (pitchc > 0)
                    {
                        if (top<48)
                        {
                            centre = centre+1;
                            pitchc = pitchc-1;
                            top=top+1;
                        }else{
                            pitchc=0;
                        }
                    }else{
                        if (bot>-18)
                        {
                            centre=centre-1;
                            pitchc=pitchc+1;
                            bot=bot-1;
                        }else{
                            pitchc=0;
                        }
                    }
                }
                    
                diff=rollc*2;
                centre=centre;
                
            //Convert to PWM           
            left_servo = left_neutral + (centre + diff) * left_scale;
            right_servo = right_neutral + (centre - diff) * right_scale;
            
            //Saturate outputs(For Safety) -18 to 48deg
            if(left_servo>0.94){left_servo=0.94;}//0.54
            if(left_servo<0.07){left_servo=0.07;}
            if(right_servo>0.93){right_servo=0.93;}
            if(right_servo<0.06){right_servo=0.06;}//0.41
            
            //Output to Servos
            servo_1 = left_servo; //0.07 - 0.59
            servo_2 = right_servo;//0.93 - 0.41
             
                        //timer.reset();
            //timer.start();
           // begin = timer.read_us();   
            //Display
            /*
            if (i>0) //5 every second at 250Hz =5Hz (50)
            {

                i=0;
            }
            */
            i++;
            j++;   
            //end = timer.read_us();
            //timee=end-begin;
         
        }    
    }
}



void display_thread(){

    while(1){
        if (tockdisp()){
            //pc.printf("(Roll %f)   (Pitch %f)         \r",roll,pitch);
            //pc.printf("%f         %f                \r",roll,pitch);
            pc.printf("%f,%f,%f,%f,%f\n", roll, pitch, yaw, (float)ain.read(), (float)ain1.read()) ;
            //pc.printf("%d         %d        %d         %f        %f           %f      %f        %f      %f      %d\n",IMU.ax,IMU.ay,IMU.az,roll, pitch,left_servo, right_servo, (float)ain.read(), (float)ain1.read()),i;
            //pc.printf("%f         %f        %f\r",P,I,D);
            //pc.printf("%f         %f        %f         %f         %f         %f\r",P,I,D,Pp,Ip,Dp);
        }
        //Thread::wait(164);
    }
}