David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

main.cpp@1:b87e95907a18, 2016-03-31 (annotated)

Committer:
ivo_david_michelle
Date:
Thu Mar 31 20:24:24 2016 +0000
Revision:
1:b87e95907a18
Parent:
0:4c04e4fd1310
Child:
2:c041e434eab6
basic integration of imu and control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ivo_david_michelle 0:4c04e4fd1310 1 #include "mbed.h"
ivo_david_michelle 1:b87e95907a18 2 #define _MBED_
ivo_david_michelle 1:b87e95907a18 3 #include "Adafruit_9DOF.h"
ivo_david_michelle 1:b87e95907a18 4 #include "Serial_base.h"
ivo_david_michelle 1:b87e95907a18 5 #include "controller.h"
ivo_david_michelle 1:b87e95907a18 6
ivo_david_michelle 1:b87e95907a18 7
ivo_david_michelle 1:b87e95907a18 8 DigitalOut myled(LED1);
ivo_david_michelle 1:b87e95907a18 9 Adafruit_9DOF dof = Adafruit_9DOF();
ivo_david_michelle 1:b87e95907a18 10 Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
ivo_david_michelle 1:b87e95907a18 11 Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(30302);
ivo_david_michelle 1:b87e95907a18 12 Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);
ivo_david_michelle 0:4c04e4fd1310 13
ivo_david_michelle 0:4c04e4fd1310 14 Serial pc(USBTX, USBRX);
ivo_david_michelle 0:4c04e4fd1310 15
ivo_david_michelle 0:4c04e4fd1310 16
ivo_david_michelle 0:4c04e4fd1310 17
ivo_david_michelle 1:b87e95907a18 18 /* Offsets of gyro at rest */
ivo_david_michelle 1:b87e95907a18 19 float x_offset = 0, y_offset = 0, z_offset = 0;
ivo_david_michelle 0:4c04e4fd1310 20
ivo_david_michelle 1:b87e95907a18 21 void initSensors()
ivo_david_michelle 1:b87e95907a18 22 {
ivo_david_michelle 1:b87e95907a18 23 s_com->println(("\r\nInitializing sensors!"));
ivo_david_michelle 1:b87e95907a18 24 if (!accel.begin()) {
ivo_david_michelle 1:b87e95907a18 25 /* There was a problem detecting the LSM303 ... check your connections */
ivo_david_michelle 1:b87e95907a18 26 s_com->println(("Ooops, no LSM303 accel detected ... Check your wiring!"));
ivo_david_michelle 1:b87e95907a18 27 while(1);
ivo_david_michelle 1:b87e95907a18 28 }
ivo_david_michelle 1:b87e95907a18 29 if (!mag.begin()) {
ivo_david_michelle 1:b87e95907a18 30 /* There was a problem detecting the LSM303 ... check your connections */
ivo_david_michelle 1:b87e95907a18 31 s_com->println("Ooops, no LSM303 mag detected ... Check your wiring!");
ivo_david_michelle 1:b87e95907a18 32 while(1);
ivo_david_michelle 1:b87e95907a18 33 }
ivo_david_michelle 1:b87e95907a18 34 if (!gyro.begin(GYRO_RANGE_2000DPS)) {
ivo_david_michelle 1:b87e95907a18 35 /* There was a problem detecting the L3GD20 ... check your connections */
ivo_david_michelle 1:b87e95907a18 36 s_com->println("Ooops, no L3GD20 gyro detected ... Check your wiring or I2C ADDR!");
ivo_david_michelle 1:b87e95907a18 37 while(1);
ivo_david_michelle 1:b87e95907a18 38 }
ivo_david_michelle 1:b87e95907a18 39 /* Calculate initial offsets and noise level of gyro */
ivo_david_michelle 1:b87e95907a18 40 float sampleNum = 500;
ivo_david_michelle 1:b87e95907a18 41 sensors_event_t gyro_event;
ivo_david_michelle 1:b87e95907a18 42 for (int n = 0; n < sampleNum; n++) {
ivo_david_michelle 1:b87e95907a18 43 gyro.getEvent(&gyro_event);
ivo_david_michelle 1:b87e95907a18 44 x_offset += gyro_event.gyro.x;
ivo_david_michelle 1:b87e95907a18 45 y_offset += gyro_event.gyro.y;
ivo_david_michelle 1:b87e95907a18 46 z_offset += gyro_event.gyro.z;
ivo_david_michelle 1:b87e95907a18 47 }
ivo_david_michelle 1:b87e95907a18 48 x_offset = x_offset / sampleNum;
ivo_david_michelle 1:b87e95907a18 49 y_offset = y_offset / sampleNum;
ivo_david_michelle 1:b87e95907a18 50 z_offset = z_offset / sampleNum;
ivo_david_michelle 1:b87e95907a18 51 s_com->print("Offsets... X: ");
ivo_david_michelle 1:b87e95907a18 52 s_com->print(x_offset);
ivo_david_michelle 1:b87e95907a18 53 s_com->print("\tY: ");
ivo_david_michelle 1:b87e95907a18 54 s_com->print(y_offset);
ivo_david_michelle 1:b87e95907a18 55 s_com->print("\tZ: ");
ivo_david_michelle 1:b87e95907a18 56 s_com->print(z_offset);
ivo_david_michelle 1:b87e95907a18 57 }
ivo_david_michelle 0:4c04e4fd1310 58
ivo_david_michelle 0:4c04e4fd1310 59
ivo_david_michelle 0:4c04e4fd1310 60
ivo_david_michelle 0:4c04e4fd1310 61 int main()
ivo_david_michelle 0:4c04e4fd1310 62 {
ivo_david_michelle 0:4c04e4fd1310 63 double F=0;
ivo_david_michelle 0:4c04e4fd1310 64 double M_x=0;
ivo_david_michelle 0:4c04e4fd1310 65 double M_y=0;
ivo_david_michelle 0:4c04e4fd1310 66 double M_z=0;
ivo_david_michelle 1:b87e95907a18 67
ivo_david_michelle 1:b87e95907a18 68 initSensors();
ivo_david_michelle 1:b87e95907a18 69
ivo_david_michelle 1:b87e95907a18 70
ivo_david_michelle 1:b87e95907a18 71 //double *attitude = (double *) malloc(6 * sizeof(double));
ivo_david_michelle 1:b87e95907a18 72 sensors_event_t accel_event;
ivo_david_michelle 1:b87e95907a18 73 sensors_event_t mag_event;
ivo_david_michelle 1:b87e95907a18 74 sensors_event_t gyro_event;
ivo_david_michelle 1:b87e95907a18 75 sensors_vec_t orientation;
ivo_david_michelle 1:b87e95907a18 76
ivo_david_michelle 0:4c04e4fd1310 77
ivo_david_michelle 0:4c04e4fd1310 78 while (1) {
ivo_david_michelle 1:b87e95907a18 79
ivo_david_michelle 1:b87e95907a18 80 /* Calculate pitch and roll from the raw accelerometer data */
ivo_david_michelle 1:b87e95907a18 81 accel.getEvent(&accel_event);
ivo_david_michelle 1:b87e95907a18 82 if (dof.accelGetOrientation(&accel_event, &orientation)) {
ivo_david_michelle 1:b87e95907a18 83 /* 'orientation' should have valid .roll and .pitch fields */
ivo_david_michelle 1:b87e95907a18 84 //s_com->print(("Roll: "));
ivo_david_michelle 1:b87e95907a18 85 //s_com->print(orientation.roll);
ivo_david_michelle 1:b87e95907a18 86 //s_com->print(("; "));
ivo_david_michelle 1:b87e95907a18 87 //s_com->print(("Pitch: "));
ivo_david_michelle 1:b87e95907a18 88 //s_com->print(orientation.pitch);
ivo_david_michelle 1:b87e95907a18 89 //s_com->print((";\t"));
ivo_david_michelle 1:b87e95907a18 90 }
ivo_david_michelle 1:b87e95907a18 91 /* Calculate the heading using the magnetometer */
ivo_david_michelle 1:b87e95907a18 92 mag.getEvent(&mag_event);
ivo_david_michelle 1:b87e95907a18 93 if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation)) {
ivo_david_michelle 1:b87e95907a18 94 /* 'orientation' should have valid .heading data now */
ivo_david_michelle 1:b87e95907a18 95 //s_com->print(("Heading: "));
ivo_david_michelle 1:b87e95907a18 96 //s_com->print(orientation.heading);
ivo_david_michelle 1:b87e95907a18 97 // s_com->print((";\r\n"));
ivo_david_michelle 1:b87e95907a18 98 }
ivo_david_michelle 1:b87e95907a18 99 /* Get angular rate data from gyroscope */
ivo_david_michelle 1:b87e95907a18 100 gyro.getEvent(&gyro_event);
ivo_david_michelle 1:b87e95907a18 101 gyro_event.gyro.x -= x_offset;
ivo_david_michelle 1:b87e95907a18 102 gyro_event.gyro.y -= y_offset;
ivo_david_michelle 1:b87e95907a18 103 gyro_event.gyro.z -= z_offset;
ivo_david_michelle 1:b87e95907a18 104
ivo_david_michelle 1:b87e95907a18 105 wait(0.1);
ivo_david_michelle 1:b87e95907a18 106
ivo_david_michelle 0:4c04e4fd1310 107 // get sensor values
ivo_david_michelle 0:4c04e4fd1310 108
ivo_david_michelle 0:4c04e4fd1310 109 // call controller
ivo_david_michelle 1:b87e95907a18 110 M_x= controller(gyro_event, orientation);
ivo_david_michelle 0:4c04e4fd1310 111
ivo_david_michelle 0:4c04e4fd1310 112 // plot
ivo_david_michelle 0:4c04e4fd1310 113
ivo_david_michelle 0:4c04e4fd1310 114 //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r", F, M_x, M_y, M_z);
ivo_david_michelle 1:b87e95907a18 115 pc.printf("M_x: %f \n\r", M_x);
ivo_david_michelle 0:4c04e4fd1310 116 }
ivo_david_michelle 1:b87e95907a18 117 //free(attitude);
ivo_david_michelle 0:4c04e4fd1310 118 }