Diff: main.cpp

Revision:

1:57378b3160b9

Parent:

0:4cf11edf30cf

diff -r 4cf11edf30cf -r 57378b3160b9 main.cpp
--- a/main.cpp	Thu Apr 11 22:24:36 2019 +0000
+++ b/main.cpp	Mon Apr 15 14:50:49 2019 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+#include "rtos.h" //for threads
 #include "LSM9DS1.h"
 #include "Servo.h"
 #include "clock.h"
@@ -6,19 +7,33 @@
 #define PI 3.14159
 #define DECLINATION -2.44
 #define deltat_ 0.004f //250Hz
+#define deltat_disp 0.164f //~6Hz
 
 //Setup Patameters
-Servo servo_1(p24); //left servo
-Servo servo_2(p25); //right servo
+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_(deltat_);
+    set_tick_disp(deltat_disp);   
 }
 
 int main()
@@ -27,6 +42,7 @@
     //Display Setup
         Timer timer;
         Timer timer2;
+
      //   int timee =0;
     int i=50;
     int j =0;
@@ -65,8 +81,6 @@
     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
@@ -81,21 +95,31 @@
         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();
-            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));
+            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;
-           // printf("%f  %f\n", roll, pitch);
+
+
+            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. 
@@ -194,15 +218,13 @@
             //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();
@@ -211,3 +233,21 @@
         }    
     }
 }
+
+
+
+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);
+    }
+}
+