Diff: main.cpp

Revision:

0:4cf11edf30cf

Child:

1:57378b3160b9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 11 22:24:36 2019 +0000
@@ -0,0 +1,213 @@
+#include "mbed.h"
+#include "LSM9DS1.h"
+#include "Servo.h"
+#include "clock.h"
+
+#define PI 3.14159
+#define DECLINATION -2.44
+#define deltat_ 0.004f //250Hz
+
+//Setup Patameters
+Servo servo_1(p24); //left servo
+Servo servo_2(p25); //right servo
+DigitalOut myled(LED1);
+Serial pc(USBTX, USBRX);
+
+AnalogIn ain(A0);
+AnalogIn ain1(A1);
+
+void Setup()
+{
+    set_tick_(deltat_);  
+}
+
+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
+    float left_servo=0;  
+    float right_servo=0;
+    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
+    
+    printf("Start");
+    while(1) 
+    {
+        if(tock())
+        {   
+
+            //Attitude     
+            while(!IMU.accelAvailable());
+            IMU.readAccel();
+            float roll = atan2((double)IMU.ax, (double)IMU.az); //swtiched the roll and pitch eqautions round
+            float 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;
+           // printf("%f  %f\n", roll, pitch);
+            
+             //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)
+            {
+                //pc.printf("(Roll %f)   (Pitch %f)         \r",roll,pitch);
+                //pc.printf("%f         %f                \r",roll,pitch);
+                pc.printf("%d         %d        %d         %f        %f      %f         %f        %f      %f        %f      %f \r",IMU.ax,IMU.ay,IMU.az,roll, pitch,roll_correct,pitch_correct,left_servo, right_servo, (float)ain.read(), (float)ain1.read());
+                //pc.printf("%f         %f        %f\r",P,I,D);
+                //pc.printf("%f         %f        %f         %f         %f         %f\r",P,I,D,Pp,Ip,Dp);
+                i=0;
+            }
+            i++;
+            j++;   
+            //end = timer.read_us();
+            //timee=end-begin;
+         
+        }    
+    }
+}