David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

main.cpp@4:3040d0f9e8c6, 2016-04-01 (annotated)

Committer:
ivo_david_michelle
Date:
Fri Apr 01 18:55:21 2016 +0000
Revision:
4:3040d0f9e8c6
Parent:
3:828e82089564
Child:
5:f007542f1dab
implemented pwm output (not finished)

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 4:3040d0f9e8c6 16 // pwm outputs for the 4 motors
ivo_david_michelle 4:3040d0f9e8c6 17 PwmOut motor_1(p21);
ivo_david_michelle 4:3040d0f9e8c6 18 PwmOut motor_2(p23);
ivo_david_michelle 4:3040d0f9e8c6 19 PwmOut motor_3(p23);
ivo_david_michelle 4:3040d0f9e8c6 20 PwmOut motor_4(p24);
ivo_david_michelle 4:3040d0f9e8c6 21
ivo_david_michelle 4:3040d0f9e8c6 22
ivo_david_michelle 4:3040d0f9e8c6 23
ivo_david_michelle 4:3040d0f9e8c6 24
ivo_david_michelle 0:4c04e4fd1310 25
ivo_david_michelle 0:4c04e4fd1310 26
ivo_david_michelle 0:4c04e4fd1310 27 int main()
ivo_david_michelle 0:4c04e4fd1310 28 {
ivo_david_michelle 3:828e82089564 29 control result;
ivo_david_michelle 4:3040d0f9e8c6 30 offset offset_gyro;
ivo_david_michelle 3:828e82089564 31
ivo_david_michelle 3:828e82089564 32 initSensors(accel, mag, gyro,offset_gyro);
ivo_david_michelle 1:b87e95907a18 33
ivo_david_michelle 1:b87e95907a18 34 sensors_event_t accel_event;
ivo_david_michelle 1:b87e95907a18 35 sensors_event_t mag_event;
ivo_david_michelle 1:b87e95907a18 36 sensors_event_t gyro_event;
ivo_david_michelle 1:b87e95907a18 37 sensors_vec_t orientation;
ivo_david_michelle 4:3040d0f9e8c6 38
ivo_david_michelle 4:3040d0f9e8c6 39 motor_1.period(0.002); // servo requires a 2ms period
ivo_david_michelle 4:3040d0f9e8c6 40 motor_2.period(0.002); // servo requires a 2ms period
ivo_david_michelle 4:3040d0f9e8c6 41 motor_3.period(0.002); // servo requires a 2ms period
ivo_david_michelle 4:3040d0f9e8c6 42 motor_4.period(0.002); // servo requires a 2ms period
ivo_david_michelle 4:3040d0f9e8c6 43
ivo_david_michelle 4:3040d0f9e8c6 44 // pwm duty cycles for the 4 motors
ivo_david_michelle 4:3040d0f9e8c6 45 motor_1 = 0;
ivo_david_michelle 4:3040d0f9e8c6 46 motor_2 = 0;
ivo_david_michelle 4:3040d0f9e8c6 47 motor_3 = 0;
ivo_david_michelle 4:3040d0f9e8c6 48 motor_4 = 0;
ivo_david_michelle 4:3040d0f9e8c6 49
ivo_david_michelle 4:3040d0f9e8c6 50 // startup procedure
ivo_david_michelle 4:3040d0f9e8c6 51
ivo_david_michelle 4:3040d0f9e8c6 52 pc.printf("Type 's' to start up Motors, or anything else to abort.\n\r");
ivo_david_michelle 4:3040d0f9e8c6 53 char a= pc.getc();
ivo_david_michelle 4:3040d0f9e8c6 54 if (a!='s') {
ivo_david_michelle 4:3040d0f9e8c6 55 pc.printf("Aborting");
ivo_david_michelle 4:3040d0f9e8c6 56 return 0;
ivo_david_michelle 4:3040d0f9e8c6 57 };
ivo_david_michelle 4:3040d0f9e8c6 58 pc.printf("Starting up ESCs\n\r");
ivo_david_michelle 4:3040d0f9e8c6 59 motor_1 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 60 motor_2 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 61 motor_3 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 62 motor_4 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 63
ivo_david_michelle 4:3040d0f9e8c6 64 pc.printf("Type 'c' to enter control loop, or anything else to abort.\n\r");
ivo_david_michelle 4:3040d0f9e8c6 65 char b= pc.getc();
ivo_david_michelle 4:3040d0f9e8c6 66 if (b!='c') {
ivo_david_michelle 4:3040d0f9e8c6 67 pc.printf("Aborting");
ivo_david_michelle 4:3040d0f9e8c6 68 return 0;
ivo_david_michelle 4:3040d0f9e8c6 69 };
ivo_david_michelle 4:3040d0f9e8c6 70
ivo_david_michelle 4:3040d0f9e8c6 71 pc.printf("Entering control loop\n\r");
ivo_david_michelle 4:3040d0f9e8c6 72
ivo_david_michelle 4:3040d0f9e8c6 73 while (1) {
ivo_david_michelle 3:828e82089564 74
ivo_david_michelle 0:4c04e4fd1310 75
ivo_david_michelle 1:b87e95907a18 76
ivo_david_michelle 1:b87e95907a18 77 /* Calculate pitch and roll from the raw accelerometer data */
ivo_david_michelle 1:b87e95907a18 78 accel.getEvent(&accel_event);
ivo_david_michelle 1:b87e95907a18 79 if (dof.accelGetOrientation(&accel_event, &orientation)) {
ivo_david_michelle 1:b87e95907a18 80 /* 'orientation' should have valid .roll and .pitch fields */
ivo_david_michelle 1:b87e95907a18 81 //s_com->print(("Roll: "));
ivo_david_michelle 1:b87e95907a18 82 //s_com->print(orientation.roll);
ivo_david_michelle 1:b87e95907a18 83 //s_com->print(("; "));
ivo_david_michelle 1:b87e95907a18 84 //s_com->print(("Pitch: "));
ivo_david_michelle 1:b87e95907a18 85 //s_com->print(orientation.pitch);
ivo_david_michelle 1:b87e95907a18 86 //s_com->print((";\t"));
ivo_david_michelle 1:b87e95907a18 87 }
ivo_david_michelle 1:b87e95907a18 88 /* Calculate the heading using the magnetometer */
ivo_david_michelle 1:b87e95907a18 89 mag.getEvent(&mag_event);
ivo_david_michelle 1:b87e95907a18 90 if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation)) {
ivo_david_michelle 1:b87e95907a18 91 /* 'orientation' should have valid .heading data now */
ivo_david_michelle 1:b87e95907a18 92 //s_com->print(("Heading: "));
ivo_david_michelle 1:b87e95907a18 93 //s_com->print(orientation.heading);
ivo_david_michelle 4:3040d0f9e8c6 94 // s_com->print((";\r\n"));
ivo_david_michelle 1:b87e95907a18 95 }
ivo_david_michelle 1:b87e95907a18 96 /* Get angular rate data from gyroscope */
ivo_david_michelle 1:b87e95907a18 97 gyro.getEvent(&gyro_event);
ivo_david_michelle 3:828e82089564 98 gyro_event.gyro.x -= offset_gyro.x_offset;
ivo_david_michelle 3:828e82089564 99 gyro_event.gyro.y -= offset_gyro.y_offset;
ivo_david_michelle 3:828e82089564 100 gyro_event.gyro.z -= offset_gyro.z_offset;
ivo_david_michelle 1:b87e95907a18 101
ivo_david_michelle 1:b87e95907a18 102 wait(0.1);
ivo_david_michelle 1:b87e95907a18 103
ivo_david_michelle 0:4c04e4fd1310 104 // get sensor values
ivo_david_michelle 0:4c04e4fd1310 105
ivo_david_michelle 0:4c04e4fd1310 106 // call controller
ivo_david_michelle 2:c041e434eab6 107 controller(gyro_event, orientation, &result);
ivo_david_michelle 0:4c04e4fd1310 108
ivo_david_michelle 4:3040d0f9e8c6 109 // compute PWM singals
ivo_david_michelle 4:3040d0f9e8c6 110 // assumption for low angles the computed moments are between -10...10
ivo_david_michelle 4:3040d0f9e8c6 111 // since I dont want to risk, i set PWM to 60% duty cycle if deflection =10.
ivo_david_michelle 4:3040d0f9e8c6 112 // USB connection of quadcopter points into x direction.
ivo_david_michelle 4:3040d0f9e8c6 113
ivo_david_michelle 4:3040d0f9e8c6 114
ivo_david_michelle 4:3040d0f9e8c6 115 // Set duty cycle
ivo_david_michelle 4:3040d0f9e8c6 116 // continue looking what pwm is.
ivo_david_michelle 4:3040d0f9e8c6 117 motor_2=50+result.M_x;
ivo_david_michelle 4:3040d0f9e8c6 118 motor_4=50-result.M_x;
ivo_david_michelle 4:3040d0f9e8c6 119
ivo_david_michelle 4:3040d0f9e8c6 120
ivo_david_michelle 0:4c04e4fd1310 121 // plot
ivo_david_michelle 0:4c04e4fd1310 122
ivo_david_michelle 0:4c04e4fd1310 123 //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 124 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 125 }
ivo_david_michelle 0:4c04e4fd1310 126 }