David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

main.cpp@2:c041e434eab6, 2016-03-31 (annotated)

Committer:
ivo_david_michelle
Date:
Thu Mar 31 20:47:04 2016 +0000
Revision:
2:c041e434eab6
Parent:
1:b87e95907a18
Child:
3:828e82089564
struct for control results

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