David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

main.cpp@14:64b06476d943, 2016-04-07 (annotated)

Committer:
ivo_david_michelle
Date:
Thu Apr 07 02:07:33 2016 +0000
Revision:
14:64b06476d943
Parent:
13:291ba30c7806
Child:
15:90e07946186f
msoch update - compiles!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ivo_david_michelle 0:4c04e4fd1310 1 #include "mbed.h"
ivo_david_michelle 14:64b06476d943 2 #include "rtos.h"
ivo_david_michelle 1:b87e95907a18 3 #define _MBED_
ivo_david_michelle 13:291ba30c7806 4 //#include "controller.h"
ivo_david_michelle 3:828e82089564 5 #include "sensor.h"
ivo_david_michelle 7:f3f94eadc5b5 6 #include "quadcopter.h"
ivo_david_michelle 7:f3f94eadc5b5 7
ivo_david_michelle 0:4c04e4fd1310 8 Serial pc(USBTX, USBRX);
ivo_david_michelle 14:64b06476d943 9 Quadcopter myQuadcopter(&pc); // instantiate Quadcopter object
ivo_david_michelle 7:f3f94eadc5b5 10
ivo_david_michelle 6:6f3ffd97d808 11 // 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 12 PwmOut motor_1(p21);
ivo_david_michelle 5:f007542f1dab 13 PwmOut motor_2(p22);
ivo_david_michelle 4:3040d0f9e8c6 14 PwmOut motor_3(p23);
ivo_david_michelle 4:3040d0f9e8c6 15 PwmOut motor_4(p24);
ivo_david_michelle 4:3040d0f9e8c6 16
ivo_david_michelle 14:64b06476d943 17 void controller_thread(void const *args) {
ivo_david_michelle 14:64b06476d943 18 while(true){
ivo_david_michelle 14:64b06476d943 19 myQuadcopter.controller();
ivo_david_michelle 14:64b06476d943 20 Thread::wait(10);
ivo_david_michelle 14:64b06476d943 21 }
ivo_david_michelle 14:64b06476d943 22 }
ivo_david_michelle 9:f1bd96708a21 23
ivo_david_michelle 14:64b06476d943 24 void rc_thread(void const *args) {
ivo_david_michelle 14:64b06476d943 25 while(true){
ivo_david_michelle 14:64b06476d943 26 myQuadcopter.readRc();
ivo_david_michelle 14:64b06476d943 27 Thread::wait(100);
ivo_david_michelle 14:64b06476d943 28 }
ivo_david_michelle 14:64b06476d943 29 }
ivo_david_michelle 14:64b06476d943 30
ivo_david_michelle 14:64b06476d943 31
ivo_david_michelle 14:64b06476d943 32 int main() {
ivo_david_michelle 14:64b06476d943 33 //Thread thread(controller_thread);
ivo_david_michelle 14:64b06476d943 34
ivo_david_michelle 14:64b06476d943 35 // get desired values from joystick (to be implemented)
ivo_david_michelle 14:64b06476d943 36 // myQuadcopter.setDes(...)
ivo_david_michelle 10:e7d1801e966a 37
ivo_david_michelle 6:6f3ffd97d808 38 motor_1.period(0.002); // motor requires a 2ms period
ivo_david_michelle 6:6f3ffd97d808 39 motor_2.period(0.002); // motor requires a 2ms period
ivo_david_michelle 6:6f3ffd97d808 40 motor_3.period(0.002); // motor requires a 2ms period
ivo_david_michelle 6:6f3ffd97d808 41 motor_4.period(0.002); // motor requires a 2ms period
ivo_david_michelle 4:3040d0f9e8c6 42
ivo_david_michelle 4:3040d0f9e8c6 43 // startup procedure
ivo_david_michelle 4:3040d0f9e8c6 44 pc.printf("Type 's' to start up Motors, or anything else to abort.\n\r");
ivo_david_michelle 4:3040d0f9e8c6 45 char a= pc.getc();
ivo_david_michelle 4:3040d0f9e8c6 46 if (a!='s') {
ivo_david_michelle 4:3040d0f9e8c6 47 pc.printf("Aborting");
ivo_david_michelle 4:3040d0f9e8c6 48 return 0;
ivo_david_michelle 4:3040d0f9e8c6 49 };
ivo_david_michelle 13:291ba30c7806 50
ivo_david_michelle 12:422963993df5 51 // Duty cycle at beginning must be 50%
ivo_david_michelle 4:3040d0f9e8c6 52 pc.printf("Starting up ESCs\n\r");
ivo_david_michelle 4:3040d0f9e8c6 53 motor_1 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 54 motor_2 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 55 motor_3 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 56 motor_4 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 57
ivo_david_michelle 4:3040d0f9e8c6 58 pc.printf("Type 'c' to enter control loop, or anything else to abort.\n\r");
ivo_david_michelle 14:64b06476d943 59 char b = pc.getc();
ivo_david_michelle 4:3040d0f9e8c6 60 if (b!='c') {
ivo_david_michelle 4:3040d0f9e8c6 61 pc.printf("Aborting");
ivo_david_michelle 4:3040d0f9e8c6 62 return 0;
ivo_david_michelle 4:3040d0f9e8c6 63 };
ivo_david_michelle 14:64b06476d943 64
ivo_david_michelle 14:64b06476d943 65 //
ivo_david_michelle 14:64b06476d943 66 Thread threadR(rc_thread);
ivo_david_michelle 14:64b06476d943 67
ivo_david_michelle 14:64b06476d943 68 //check if remote controll is working with if statement
ivo_david_michelle 14:64b06476d943 69
ivo_david_michelle 14:64b06476d943 70 Thread threadC(controller_thread);
ivo_david_michelle 4:3040d0f9e8c6 71
ivo_david_michelle 4:3040d0f9e8c6 72 pc.printf("Entering control loop\n\r");
ivo_david_michelle 4:3040d0f9e8c6 73
ivo_david_michelle 4:3040d0f9e8c6 74 while (1) {
ivo_david_michelle 11:5c54826d23a7 75 myQuadcopter.readSensorValues();
ivo_david_michelle 11:5c54826d23a7 76
ivo_david_michelle 14:64b06476d943 77 // myQuadcopter.controller();
ivo_david_michelle 13:291ba30c7806 78
ivo_david_michelle 13:291ba30c7806 79
ivo_david_michelle 5:f007542f1dab 80 wait(0.01);
ivo_david_michelle 1:b87e95907a18 81
ivo_david_michelle 4:3040d0f9e8c6 82 // Set duty cycle
ivo_david_michelle 11:5c54826d23a7 83 motors motorsPwm=myQuadcopter.getPwm();
ivo_david_michelle 5:f007542f1dab 84
ivo_david_michelle 11:5c54826d23a7 85 motor_2=motorsPwm.m2;
ivo_david_michelle 11:5c54826d23a7 86 motor_4=motorsPwm.m4;
ivo_david_michelle 14:64b06476d943 87 //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 4:3040d0f9e8c6 88
ivo_david_michelle 14:64b06476d943 89 pc.printf("m1: %f m2: %f \n\r", motorsPwm.m2, motorsPwm.m4);
ivo_david_michelle 11:5c54826d23a7 90 //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 91 }
ivo_david_michelle 0:4c04e4fd1310 92 }