Diff: main.cpp

Revision:

1:b87e95907a18

Parent:

0:4c04e4fd1310

Child:

2:c041e434eab6

--- a/main.cpp	Wed Mar 30 16:05:30 2016 +0000
+++ b/main.cpp	Thu Mar 31 20:24:24 2016 +0000
@@ -1,81 +1,62 @@
 #include "mbed.h"
+#define _MBED_
+#include "Adafruit_9DOF.h"
+#include "Serial_base.h"
+#include "controller.h"
+
+
+DigitalOut myled(LED1);
+Adafruit_9DOF dof = Adafruit_9DOF();
+Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
+Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(30302);
+Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);
 
 Serial pc(USBTX, USBRX);
 
-double controller(double phi)
-{
-    double g=9.81;  // gravity [m/s^2]
-    double m=1; // mass [kg]
 
-    // proportional attitude control gains
-    double kp_phi = 1;
-    double kp_theta = 1;
-    double kp_psi = 1;
-
-    // derivative attitude control gains
-    double kd_phi = 0;
-    double kd_theta = 0;
-    double kd_psi = 0;
 
-    // measured values (will come from IMU/parameter class/Input to function later)
-    // double phi = 0; // roll // commented out, because input to function
-    double theta = 0; // pitch
-    double psi = 0; // yaw
-
-    // angular velocities in body coordinate system
-    double p = 0; // omega_x
-    double q = 0;  // omega_y
-    double r = 0;  // omega_z
-
-    // desired values
-    double F_des = 0; // desired thrust force (excluding weight compensation)
-    double phi_des = 0; // roll
-    double theta_des = 0; // pitch
-    double psi_des = 0; // yaw
-    // these will probably not be changed -> we could use const keyword in case of storage shortage
-    double p_des = 0; // omega_x
-    double q_des = 0;  // omega_y
-    double r_des = 0;  // omega_z
+/* Offsets of gyro at rest */
+float x_offset = 0, y_offset = 0, z_offset = 0;
 
-    // total thrust force
-    double F=0;
-    // control moments
-    double M_x=0;
-    double M_y=0;
-    double M_z=0;
-
-    // get IMU data
-    // phi = 1;
-    theta =0;
-    psi =0;
-    p = 0;
-    q = 0;
-    r = 0;
-
-    // compute desired angles (in the case we decide not to set
-    // the angles, but for instance the velocity with the Joystick
+void initSensors()
+{
+    s_com->println(("\r\nInitializing sensors!"));
+    if (!accel.begin()) {
+        /* There was a problem detecting the LSM303 ... check your connections */
+        s_com->println(("Ooops, no LSM303 accel detected ... Check your wiring!"));
+        while(1);
+    }
+    if (!mag.begin()) {
+        /* There was a problem detecting the LSM303 ... check your connections */
+        s_com->println("Ooops, no LSM303 mag detected ... Check your wiring!");
+        while(1);
+    }
+    if (!gyro.begin(GYRO_RANGE_2000DPS)) {
+        /* There was a problem detecting the L3GD20 ... check your connections */
+        s_com->println("Ooops, no L3GD20 gyro detected ... Check your wiring or I2C ADDR!");
+        while(1);
+    }
+    /* Calculate initial offsets and noise level of gyro */
+    float sampleNum = 500;
+    sensors_event_t gyro_event;
+    for (int n = 0; n < sampleNum; n++) {
+        gyro.getEvent(&gyro_event);
+        x_offset += gyro_event.gyro.x;
+        y_offset += gyro_event.gyro.y;
+        z_offset += gyro_event.gyro.z;
+    }
+    x_offset = x_offset / sampleNum;
+    y_offset = y_offset / sampleNum;
+    z_offset = z_offset / sampleNum;
+    s_com->print("Offsets... X: ");
+    s_com->print(x_offset);
+    s_com->print("\tY: ");
+    s_com->print(y_offset);
+    s_com->print("\tZ: ");
+    s_com->print(z_offset);
+}
 
-    // desired values
-    // dirctly from Joystick, or compute from desired velocities.
-    F_des=1;
-    phi_des = 0;
-    theta_des =1;
-    psi_des =0;
 
-    // PD controller
-    F = m*g + F_des;
-    M_x = kp_phi*(phi_des-phi)+kd_phi*(p_des-p);
-    M_y = kp_theta*(theta_des-theta)+kd_theta*(q_des-q);
-    M_z = kp_psi*(psi_des-psi)+kd_psi*(r_des-r);
-
-    //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r",  F, M_x, M_y, M_z);
-    //        pc.printf("F: %f\n\r",  F);
-
-    //  pc.printf("Mx: %f\n\r",  F);
-    //pc.printf("F: %f\n\r",  F);
-
-    return M_x;
-}
 
 int main()
 {
@@ -83,20 +64,55 @@
     double M_x=0;
     double M_y=0;
     double M_z=0;
-    
-    double *orientation = (double *) malloc(6 * sizeof(double));
+
+    initSensors();
+
+
+    //double *attitude = (double *) malloc(6 * sizeof(double));
+    sensors_event_t accel_event;
+    sensors_event_t mag_event;
+    sensors_event_t gyro_event;
+    sensors_vec_t   orientation;
+
 
     while (1) {
+
+        /* Calculate pitch and roll from the raw accelerometer data */
+        accel.getEvent(&accel_event);
+        if (dof.accelGetOrientation(&accel_event, &orientation)) {
+            /* 'orientation' should have valid .roll and .pitch fields */
+            //s_com->print(("Roll: "));
+            //s_com->print(orientation.roll);
+            //s_com->print(("; "));
+            //s_com->print(("Pitch: "));
+            //s_com->print(orientation.pitch);
+            //s_com->print((";\t"));
+        }
+        /* Calculate the heading using the magnetometer */
+        mag.getEvent(&mag_event);
+        if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation)) {
+            /* 'orientation' should have valid .heading data now */
+            //s_com->print(("Heading: "));
+            //s_com->print(orientation.heading);
+          //  s_com->print((";\r\n"));
+        }
+        /* Get angular rate data from gyroscope */
+        gyro.getEvent(&gyro_event);
+        gyro_event.gyro.x -= x_offset;
+        gyro_event.gyro.y -= y_offset;
+        gyro_event.gyro.z -= z_offset;
+
+        wait(0.1);
+
         // get sensor values
-        double phi=1;
 
         // call controller
-        M_x= controller(phi);
+        M_x= controller(gyro_event, orientation);
 
         // plot
 
         //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r",  F, M_x, M_y, M_z);
-        pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r",  F, M_x, M_y, M_z);
+        pc.printf("M_x: %f \n\r", M_x);
     }
-    free(orientation);
+    //free(attitude);
 }