David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

main.cpp@3:828e82089564, 2016-03-31 (annotated)

Committer:
ivo_david_michelle
Date:
Thu Mar 31 22:05:57 2016 +0000
Revision:
3:828e82089564
Parent:
2:c041e434eab6
Child:
4:3040d0f9e8c6
more refactoring of code

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 3:828e82089564 6 #include "sensor.h"
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 0:4c04e4fd1310 18 int main()
ivo_david_michelle 0:4c04e4fd1310 19 {
ivo_david_michelle 3:828e82089564 20 control result;
ivo_david_michelle 3:828e82089564 21 offset offset_gyro;
ivo_david_michelle 3:828e82089564 22
ivo_david_michelle 3:828e82089564 23 initSensors(accel, mag, gyro,offset_gyro);
ivo_david_michelle 1:b87e95907a18 24
ivo_david_michelle 1:b87e95907a18 25 sensors_event_t accel_event;
ivo_david_michelle 1:b87e95907a18 26 sensors_event_t mag_event;
ivo_david_michelle 1:b87e95907a18 27 sensors_event_t gyro_event;
ivo_david_michelle 1:b87e95907a18 28 sensors_vec_t orientation;
ivo_david_michelle 2:c041e434eab6 29
ivo_david_michelle 3:828e82089564 30
ivo_david_michelle 0:4c04e4fd1310 31
ivo_david_michelle 0:4c04e4fd1310 32 while (1) {
ivo_david_michelle 1:b87e95907a18 33
ivo_david_michelle 1:b87e95907a18 34 /* Calculate pitch and roll from the raw accelerometer data */
ivo_david_michelle 1:b87e95907a18 35 accel.getEvent(&accel_event);
ivo_david_michelle 1:b87e95907a18 36 if (dof.accelGetOrientation(&accel_event, &orientation)) {
ivo_david_michelle 1:b87e95907a18 37 /* 'orientation' should have valid .roll and .pitch fields */
ivo_david_michelle 1:b87e95907a18 38 //s_com->print(("Roll: "));
ivo_david_michelle 1:b87e95907a18 39 //s_com->print(orientation.roll);
ivo_david_michelle 1:b87e95907a18 40 //s_com->print(("; "));
ivo_david_michelle 1:b87e95907a18 41 //s_com->print(("Pitch: "));
ivo_david_michelle 1:b87e95907a18 42 //s_com->print(orientation.pitch);
ivo_david_michelle 1:b87e95907a18 43 //s_com->print((";\t"));
ivo_david_michelle 1:b87e95907a18 44 }
ivo_david_michelle 1:b87e95907a18 45 /* Calculate the heading using the magnetometer */
ivo_david_michelle 1:b87e95907a18 46 mag.getEvent(&mag_event);
ivo_david_michelle 1:b87e95907a18 47 if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation)) {
ivo_david_michelle 1:b87e95907a18 48 /* 'orientation' should have valid .heading data now */
ivo_david_michelle 1:b87e95907a18 49 //s_com->print(("Heading: "));
ivo_david_michelle 1:b87e95907a18 50 //s_com->print(orientation.heading);
ivo_david_michelle 1:b87e95907a18 51 // s_com->print((";\r\n"));
ivo_david_michelle 1:b87e95907a18 52 }
ivo_david_michelle 1:b87e95907a18 53 /* Get angular rate data from gyroscope */
ivo_david_michelle 1:b87e95907a18 54 gyro.getEvent(&gyro_event);
ivo_david_michelle 3:828e82089564 55 gyro_event.gyro.x -= offset_gyro.x_offset;
ivo_david_michelle 3:828e82089564 56 gyro_event.gyro.y -= offset_gyro.y_offset;
ivo_david_michelle 3:828e82089564 57 gyro_event.gyro.z -= offset_gyro.z_offset;
ivo_david_michelle 1:b87e95907a18 58
ivo_david_michelle 1:b87e95907a18 59 wait(0.1);
ivo_david_michelle 1:b87e95907a18 60
ivo_david_michelle 0:4c04e4fd1310 61 // get sensor values
ivo_david_michelle 0:4c04e4fd1310 62
ivo_david_michelle 0:4c04e4fd1310 63 // call controller
ivo_david_michelle 2:c041e434eab6 64 controller(gyro_event, orientation, &result);
ivo_david_michelle 0:4c04e4fd1310 65
ivo_david_michelle 0:4c04e4fd1310 66 // plot
ivo_david_michelle 0:4c04e4fd1310 67
ivo_david_michelle 0:4c04e4fd1310 68 //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 69 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 70 }
ivo_david_michelle 0:4c04e4fd1310 71 }