David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

main.cpp@12:422963993df5, 2016-04-02 (annotated)

Committer:
ivo_david_michelle
Date:
Sat Apr 02 21:40:37 2016 +0000
Revision:
12:422963993df5
Parent:
11:5c54826d23a7
Child:
13:291ba30c7806
working stand with x axis control and quadcopter class

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 12:422963993df5 10 // initialze serial connection
ivo_david_michelle 0:4c04e4fd1310 11 Serial pc(USBTX, USBRX);
ivo_david_michelle 12:422963993df5 12 Serial *pcPntr=&pc; // pointer used to enable serial connection with print method in Quadcopter class
ivo_david_michelle 7:f3f94eadc5b5 13
ivo_david_michelle 6:6f3ffd97d808 14 // 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 15 PwmOut motor_1(p21);
ivo_david_michelle 5:f007542f1dab 16 PwmOut motor_2(p22);
ivo_david_michelle 4:3040d0f9e8c6 17 PwmOut motor_3(p23);
ivo_david_michelle 4:3040d0f9e8c6 18 PwmOut motor_4(p24);
ivo_david_michelle 4:3040d0f9e8c6 19
ivo_david_michelle 0:4c04e4fd1310 20 int main()
ivo_david_michelle 0:4c04e4fd1310 21 {
ivo_david_michelle 9:f1bd96708a21 22 Quadcopter myQuadcopter; // initialze Quadcopter object
ivo_david_michelle 9:f1bd96708a21 23 myQuadcopter.setSerial(pcPntr); // give Quadcopter object pc Pointer in order to print
ivo_david_michelle 9:f1bd96708a21 24 myQuadcopter.initAllSensors(); // initialize sensors
ivo_david_michelle 9:f1bd96708a21 25
ivo_david_michelle 12:422963993df5 26 // get desired values from joystick (to be implemented)
ivo_david_michelle 10:e7d1801e966a 27 // myQuadcopter.setDes(...)
ivo_david_michelle 10:e7d1801e966a 28
ivo_david_michelle 6:6f3ffd97d808 29 motor_1.period(0.002); // motor requires a 2ms period
ivo_david_michelle 6:6f3ffd97d808 30 motor_2.period(0.002); // motor requires a 2ms period
ivo_david_michelle 6:6f3ffd97d808 31 motor_3.period(0.002); // motor requires a 2ms period
ivo_david_michelle 6:6f3ffd97d808 32 motor_4.period(0.002); // motor requires a 2ms period
ivo_david_michelle 4:3040d0f9e8c6 33
ivo_david_michelle 4:3040d0f9e8c6 34 // startup procedure
ivo_david_michelle 4:3040d0f9e8c6 35 pc.printf("Type 's' to start up Motors, or anything else to abort.\n\r");
ivo_david_michelle 4:3040d0f9e8c6 36 char a= pc.getc();
ivo_david_michelle 4:3040d0f9e8c6 37 if (a!='s') {
ivo_david_michelle 4:3040d0f9e8c6 38 pc.printf("Aborting");
ivo_david_michelle 4:3040d0f9e8c6 39 return 0;
ivo_david_michelle 4:3040d0f9e8c6 40 };
ivo_david_michelle 12:422963993df5 41
ivo_david_michelle 12:422963993df5 42 // Duty cycle at beginning must be 50%
ivo_david_michelle 4:3040d0f9e8c6 43 pc.printf("Starting up ESCs\n\r");
ivo_david_michelle 4:3040d0f9e8c6 44 motor_1 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 45 motor_2 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 46 motor_3 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 47 motor_4 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 48
ivo_david_michelle 4:3040d0f9e8c6 49 pc.printf("Type 'c' to enter control loop, or anything else to abort.\n\r");
ivo_david_michelle 4:3040d0f9e8c6 50 char b= pc.getc();
ivo_david_michelle 4:3040d0f9e8c6 51 if (b!='c') {
ivo_david_michelle 4:3040d0f9e8c6 52 pc.printf("Aborting");
ivo_david_michelle 4:3040d0f9e8c6 53 return 0;
ivo_david_michelle 4:3040d0f9e8c6 54 };
ivo_david_michelle 4:3040d0f9e8c6 55
ivo_david_michelle 4:3040d0f9e8c6 56 pc.printf("Entering control loop\n\r");
ivo_david_michelle 4:3040d0f9e8c6 57
ivo_david_michelle 4:3040d0f9e8c6 58 while (1) {
ivo_david_michelle 11:5c54826d23a7 59 myQuadcopter.readSensorValues();
ivo_david_michelle 11:5c54826d23a7 60
ivo_david_michelle 11:5c54826d23a7 61 myQuadcopter.controller();
ivo_david_michelle 12:422963993df5 62
ivo_david_michelle 12:422963993df5 63
ivo_david_michelle 5:f007542f1dab 64 wait(0.01);
ivo_david_michelle 1:b87e95907a18 65
ivo_david_michelle 4:3040d0f9e8c6 66 // Set duty cycle
ivo_david_michelle 11:5c54826d23a7 67 motors motorsPwm=myQuadcopter.getPwm();
ivo_david_michelle 5:f007542f1dab 68
ivo_david_michelle 11:5c54826d23a7 69 motor_2=motorsPwm.m2;
ivo_david_michelle 11:5c54826d23a7 70 motor_4=motorsPwm.m4;
ivo_david_michelle 4:3040d0f9e8c6 71
ivo_david_michelle 0:4c04e4fd1310 72 //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 73 //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 74 }
ivo_david_michelle 0:4c04e4fd1310 75 }