David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

main.cpp@11:5c54826d23a7, 2016-04-02 (annotated)

Committer:
ivo_david_michelle
Date:
Sat Apr 02 18:01:36 2016 +0000
Revision:
11:5c54826d23a7
Parent:
10:e7d1801e966a
Child:
12:422963993df5
still one compiler error;

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 7:f3f94eadc5b5 8 #include "quadcopter.h"
ivo_david_michelle 7:f3f94eadc5b5 9
ivo_david_michelle 1:b87e95907a18 10 DigitalOut myled(LED1);
ivo_david_michelle 1:b87e95907a18 11 Adafruit_9DOF dof = Adafruit_9DOF();
ivo_david_michelle 1:b87e95907a18 12 Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
ivo_david_michelle 1:b87e95907a18 13 Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(30302);
ivo_david_michelle 1:b87e95907a18 14 Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);
ivo_david_michelle 0:4c04e4fd1310 15
ivo_david_michelle 0:4c04e4fd1310 16 Serial pc(USBTX, USBRX);
ivo_david_michelle 9:f1bd96708a21 17 Serial *pcPntr=&pc;
ivo_david_michelle 7:f3f94eadc5b5 18
ivo_david_michelle 6:6f3ffd97d808 19 // pwm outputs for the 4 motors (motor 1 points into x direction, 2 into y direction, 3 -x direction, -y direction
ivo_david_michelle 4:3040d0f9e8c6 20 PwmOut motor_1(p21);
ivo_david_michelle 5:f007542f1dab 21 PwmOut motor_2(p22);
ivo_david_michelle 4:3040d0f9e8c6 22 PwmOut motor_3(p23);
ivo_david_michelle 4:3040d0f9e8c6 23 PwmOut motor_4(p24);
ivo_david_michelle 4:3040d0f9e8c6 24
ivo_david_michelle 7:f3f94eadc5b5 25
ivo_david_michelle 7:f3f94eadc5b5 26 /*struct pwmOut {
ivo_david_michelle 7:f3f94eadc5b5 27 // pwmOut: this struct, PwmOut: class defined by mbed, _pwmOut: class memeber of quadcopter class
ivo_david_michelle 7:f3f94eadc5b5 28 // pwm outputs for the 4 motors (motor 1 points into x direction, 2 into y direction, 3 -x direction, -y direction
ivo_david_michelle 7:f3f94eadc5b5 29 PwmOut _motr;
ivo_david_michelle 7:f3f94eadc5b5 30 PwmOut b
ivo_david_michelle 7:f3f94eadc5b5 31 PwmOut c;
ivo_david_michelle 7:f3f94eadc5b5 32 PwmOut d;
ivo_david_michelle 7:f3f94eadc5b5 33 };
ivo_david_michelle 7:f3f94eadc5b5 34 */
ivo_david_michelle 7:f3f94eadc5b5 35
ivo_david_michelle 0:4c04e4fd1310 36 int main()
ivo_david_michelle 0:4c04e4fd1310 37 {
ivo_david_michelle 9:f1bd96708a21 38 Quadcopter myQuadcopter; // initialze Quadcopter object
ivo_david_michelle 9:f1bd96708a21 39 myQuadcopter.setSerial(pcPntr); // give Quadcopter object pc Pointer in order to print
ivo_david_michelle 9:f1bd96708a21 40 myQuadcopter.initAllSensors(); // initialize sensors
ivo_david_michelle 9:f1bd96708a21 41
ivo_david_michelle 10:e7d1801e966a 42 // get desired values from joystick
ivo_david_michelle 10:e7d1801e966a 43 // myQuadcopter.setDes(...)
ivo_david_michelle 10:e7d1801e966a 44
ivo_david_michelle 10:e7d1801e966a 45
ivo_david_michelle 11:5c54826d23a7 46 // control result;
ivo_david_michelle 11:5c54826d23a7 47 //offset offset_gyro;
ivo_david_michelle 3:828e82089564 48
ivo_david_michelle 11:5c54826d23a7 49 //initSensors(accel, mag, gyro,offset_gyro);
ivo_david_michelle 1:b87e95907a18 50
ivo_david_michelle 11:5c54826d23a7 51 /* sensors_event_t accel_event;
ivo_david_michelle 11:5c54826d23a7 52 sensors_event_t mag_event;
ivo_david_michelle 11:5c54826d23a7 53 sensors_event_t gyro_event;
ivo_david_michelle 11:5c54826d23a7 54 sensors_vec_t orientation;
ivo_david_michelle 11:5c54826d23a7 55 */
ivo_david_michelle 6:6f3ffd97d808 56 motor_1.period(0.002); // motor requires a 2ms period
ivo_david_michelle 6:6f3ffd97d808 57 motor_2.period(0.002); // motor requires a 2ms period
ivo_david_michelle 6:6f3ffd97d808 58 motor_3.period(0.002); // motor requires a 2ms period
ivo_david_michelle 6:6f3ffd97d808 59 motor_4.period(0.002); // motor requires a 2ms period
ivo_david_michelle 4:3040d0f9e8c6 60
ivo_david_michelle 4:3040d0f9e8c6 61 // pwm duty cycles for the 4 motors
ivo_david_michelle 4:3040d0f9e8c6 62 motor_1 = 0;
ivo_david_michelle 4:3040d0f9e8c6 63 motor_2 = 0;
ivo_david_michelle 4:3040d0f9e8c6 64 motor_3 = 0;
ivo_david_michelle 4:3040d0f9e8c6 65 motor_4 = 0;
ivo_david_michelle 4:3040d0f9e8c6 66
ivo_david_michelle 4:3040d0f9e8c6 67 // startup procedure
ivo_david_michelle 4:3040d0f9e8c6 68 pc.printf("Type 's' to start up Motors, or anything else to abort.\n\r");
ivo_david_michelle 4:3040d0f9e8c6 69 char a= pc.getc();
ivo_david_michelle 4:3040d0f9e8c6 70 if (a!='s') {
ivo_david_michelle 4:3040d0f9e8c6 71 pc.printf("Aborting");
ivo_david_michelle 4:3040d0f9e8c6 72 return 0;
ivo_david_michelle 4:3040d0f9e8c6 73 };
ivo_david_michelle 4:3040d0f9e8c6 74 pc.printf("Starting up ESCs\n\r");
ivo_david_michelle 4:3040d0f9e8c6 75 motor_1 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 76 motor_2 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 77 motor_3 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 78 motor_4 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 79
ivo_david_michelle 4:3040d0f9e8c6 80 pc.printf("Type 'c' to enter control loop, or anything else to abort.\n\r");
ivo_david_michelle 4:3040d0f9e8c6 81 char b= pc.getc();
ivo_david_michelle 4:3040d0f9e8c6 82 if (b!='c') {
ivo_david_michelle 4:3040d0f9e8c6 83 pc.printf("Aborting");
ivo_david_michelle 4:3040d0f9e8c6 84 return 0;
ivo_david_michelle 4:3040d0f9e8c6 85 };
ivo_david_michelle 4:3040d0f9e8c6 86
ivo_david_michelle 4:3040d0f9e8c6 87 pc.printf("Entering control loop\n\r");
ivo_david_michelle 4:3040d0f9e8c6 88
ivo_david_michelle 4:3040d0f9e8c6 89 while (1) {
ivo_david_michelle 11:5c54826d23a7 90 myQuadcopter.readSensorValues();
ivo_david_michelle 11:5c54826d23a7 91
ivo_david_michelle 11:5c54826d23a7 92 myQuadcopter.controller();
ivo_david_michelle 11:5c54826d23a7 93 /*
ivo_david_michelle 1:b87e95907a18 94 accel.getEvent(&accel_event);
ivo_david_michelle 1:b87e95907a18 95 if (dof.accelGetOrientation(&accel_event, &orientation)) {
ivo_david_michelle 1:b87e95907a18 96 //s_com->print(("Roll: "));
ivo_david_michelle 1:b87e95907a18 97 //s_com->print(orientation.roll);
ivo_david_michelle 1:b87e95907a18 98 //s_com->print(("; "));
ivo_david_michelle 1:b87e95907a18 99 //s_com->print(("Pitch: "));
ivo_david_michelle 1:b87e95907a18 100 //s_com->print(orientation.pitch);
ivo_david_michelle 1:b87e95907a18 101 //s_com->print((";\t"));
ivo_david_michelle 1:b87e95907a18 102 }
ivo_david_michelle 11:5c54826d23a7 103 */
ivo_david_michelle 11:5c54826d23a7 104
ivo_david_michelle 1:b87e95907a18 105 /* Calculate the heading using the magnetometer */
ivo_david_michelle 11:5c54826d23a7 106 /*mag.getEvent(&mag_event);
ivo_david_michelle 1:b87e95907a18 107 if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation)) {
ivo_david_michelle 1:b87e95907a18 108 //s_com->print(("Heading: "));
ivo_david_michelle 1:b87e95907a18 109 //s_com->print(orientation.heading);
ivo_david_michelle 4:3040d0f9e8c6 110 // s_com->print((";\r\n"));
ivo_david_michelle 1:b87e95907a18 111 }
ivo_david_michelle 1:b87e95907a18 112 /* Get angular rate data from gyroscope */
ivo_david_michelle 11:5c54826d23a7 113
ivo_david_michelle 11:5c54826d23a7 114 /*gyro.getEvent(&gyro_event);
ivo_david_michelle 10:e7d1801e966a 115 gyro_event.gyro.x -= offset_gyro.x;
ivo_david_michelle 10:e7d1801e966a 116 gyro_event.gyro.y -= offset_gyro.y;
ivo_david_michelle 10:e7d1801e966a 117 gyro_event.gyro.z -= offset_gyro.z;
ivo_david_michelle 11:5c54826d23a7 118 */
ivo_david_michelle 5:f007542f1dab 119 wait(0.01);
ivo_david_michelle 1:b87e95907a18 120
ivo_david_michelle 0:4c04e4fd1310 121 // get sensor values
ivo_david_michelle 0:4c04e4fd1310 122
ivo_david_michelle 0:4c04e4fd1310 123 // call controller
ivo_david_michelle 11:5c54826d23a7 124 // controller(gyro_event, orientation, &result);
ivo_david_michelle 0:4c04e4fd1310 125
ivo_david_michelle 4:3040d0f9e8c6 126 // compute PWM singals
ivo_david_michelle 4:3040d0f9e8c6 127 // assumption for low angles the computed moments are between -10...10
ivo_david_michelle 4:3040d0f9e8c6 128 // since I dont want to risk, i set PWM to 60% duty cycle if deflection =10.
ivo_david_michelle 4:3040d0f9e8c6 129 // USB connection of quadcopter points into x direction.
ivo_david_michelle 9:f1bd96708a21 130
ivo_david_michelle 9:f1bd96708a21 131
ivo_david_michelle 4:3040d0f9e8c6 132 // Set duty cycle
ivo_david_michelle 9:f1bd96708a21 133 // continue looking what pwm is.
ivo_david_michelle 11:5c54826d23a7 134 motors motorsPwm=myQuadcopter.getPwm();
ivo_david_michelle 5:f007542f1dab 135
ivo_david_michelle 11:5c54826d23a7 136 motor_2=motorsPwm.m2;
ivo_david_michelle 11:5c54826d23a7 137 motor_4=motorsPwm.m4;
ivo_david_michelle 4:3040d0f9e8c6 138
ivo_david_michelle 4:3040d0f9e8c6 139
ivo_david_michelle 0:4c04e4fd1310 140 // plot
ivo_david_michelle 0:4c04e4fd1310 141
ivo_david_michelle 0:4c04e4fd1310 142 //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 11:5c54826d23a7 143 //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 144 }
ivo_david_michelle 0:4c04e4fd1310 145 }