David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

main.cpp@21:336faf452989, 2016-04-10 (annotated)

Committer:
ivo_david_michelle
Date:
Sun Apr 10 21:47:17 2016 +0000
Revision:
21:336faf452989
Parent:
20:efa15ed008b4
Child:
22:92401a4fec13
control of all 3 dof works

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 15:90e07946186f 9 MRF24J40 mrf(p11, p12, p13, p14, p21);
ivo_david_michelle 15:90e07946186f 10 Quadcopter myQuadcopter(&pc, &mrf); // instantiate Quadcopter object
ivo_david_michelle 7:f3f94eadc5b5 11
ivo_david_michelle 6:6f3ffd97d808 12 // pwm outputs for the 4 motors (motor 1 points into x direction, 2 into y direction, 3 -x direction, -y direction
ivo_david_michelle 15:90e07946186f 13 PwmOut motor_1(p23);
ivo_david_michelle 15:90e07946186f 14 PwmOut motor_2(p24);
ivo_david_michelle 15:90e07946186f 15 PwmOut motor_3(p25);
ivo_david_michelle 15:90e07946186f 16 PwmOut motor_4(p26);
ivo_david_michelle 15:90e07946186f 17
ivo_david_michelle 4:3040d0f9e8c6 18
ivo_david_michelle 15:90e07946186f 19 void controller_thread(void const *args)
ivo_david_michelle 15:90e07946186f 20 {
ivo_david_michelle 15:90e07946186f 21 while(true) {
ivo_david_michelle 19:39c144ca2a2f 22 //myQuadcopter.readSensorValues();
ivo_david_michelle 19:39c144ca2a2f 23
ivo_david_michelle 15:90e07946186f 24 myQuadcopter.controller();
ivo_david_michelle 18:a00d6b065c6b 25 motors motorsPwm=myQuadcopter.getPwm();
ivo_david_michelle 18:a00d6b065c6b 26
ivo_david_michelle 21:336faf452989 27 motor_1=motorsPwm.m1;
ivo_david_michelle 21:336faf452989 28 motor_2=motorsPwm.m2;
ivo_david_michelle 21:336faf452989 29 motor_3=motorsPwm.m3;
ivo_david_michelle 18:a00d6b065c6b 30 motor_4=motorsPwm.m4;
ivo_david_michelle 18:a00d6b065c6b 31
ivo_david_michelle 21:336faf452989 32 // pc.printf("m1: %f m2: %f m3: %f m4: %f \n\r", motorsPwm.m1, motorsPwm.m2, motorsPwm.m3, motorsPwm.m4);
ivo_david_michelle 18:a00d6b065c6b 33
ivo_david_michelle 15:90e07946186f 34 Thread::wait(10);
ivo_david_michelle 15:90e07946186f 35 }
ivo_david_michelle 14:64b06476d943 36 }
ivo_david_michelle 9:f1bd96708a21 37
ivo_david_michelle 15:90e07946186f 38 void rc_thread(void const *args)
ivo_david_michelle 15:90e07946186f 39 {
ivo_david_michelle 15:90e07946186f 40 while(true) {
ivo_david_michelle 15:90e07946186f 41 myQuadcopter.readRc();
ivo_david_michelle 18:a00d6b065c6b 42 Thread::wait(50); // wait for some number of miliseconds
ivo_david_michelle 15:90e07946186f 43 }
ivo_david_michelle 14:64b06476d943 44 }
ivo_david_michelle 14:64b06476d943 45
ivo_david_michelle 14:64b06476d943 46
ivo_david_michelle 15:90e07946186f 47 int main()
ivo_david_michelle 15:90e07946186f 48 {
ivo_david_michelle 14:64b06476d943 49 //Thread thread(controller_thread);
ivo_david_michelle 14:64b06476d943 50
ivo_david_michelle 14:64b06476d943 51 // get desired values from joystick (to be implemented)
ivo_david_michelle 14:64b06476d943 52 // myQuadcopter.setDes(...)
ivo_david_michelle 10:e7d1801e966a 53
ivo_david_michelle 6:6f3ffd97d808 54 motor_1.period(0.002); // motor requires a 2ms period
ivo_david_michelle 6:6f3ffd97d808 55 motor_2.period(0.002); // motor requires a 2ms period
ivo_david_michelle 6:6f3ffd97d808 56 motor_3.period(0.002); // motor requires a 2ms period
ivo_david_michelle 6:6f3ffd97d808 57 motor_4.period(0.002); // motor requires a 2ms period
ivo_david_michelle 4:3040d0f9e8c6 58
ivo_david_michelle 4:3040d0f9e8c6 59 // startup procedure
ivo_david_michelle 4:3040d0f9e8c6 60 pc.printf("Type 's' to start up Motors, or anything else to abort.\n\r");
ivo_david_michelle 4:3040d0f9e8c6 61 char a= pc.getc();
ivo_david_michelle 4:3040d0f9e8c6 62 if (a!='s') {
ivo_david_michelle 4:3040d0f9e8c6 63 pc.printf("Aborting");
ivo_david_michelle 4:3040d0f9e8c6 64 return 0;
ivo_david_michelle 4:3040d0f9e8c6 65 };
ivo_david_michelle 13:291ba30c7806 66
ivo_david_michelle 12:422963993df5 67 // Duty cycle at beginning must be 50%
ivo_david_michelle 4:3040d0f9e8c6 68 pc.printf("Starting up ESCs\n\r");
ivo_david_michelle 4:3040d0f9e8c6 69 motor_1 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 70 motor_2 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 71 motor_3 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 72 motor_4 = 0.5;
ivo_david_michelle 4:3040d0f9e8c6 73
ivo_david_michelle 4:3040d0f9e8c6 74 pc.printf("Type 'c' to enter control loop, or anything else to abort.\n\r");
ivo_david_michelle 14:64b06476d943 75 char b = pc.getc();
ivo_david_michelle 4:3040d0f9e8c6 76 if (b!='c') {
ivo_david_michelle 4:3040d0f9e8c6 77 pc.printf("Aborting");
ivo_david_michelle 4:3040d0f9e8c6 78 return 0;
ivo_david_michelle 4:3040d0f9e8c6 79 };
ivo_david_michelle 15:90e07946186f 80
ivo_david_michelle 14:64b06476d943 81 Thread threadR(rc_thread);
ivo_david_michelle 14:64b06476d943 82
ivo_david_michelle 14:64b06476d943 83 //check if remote controll is working with if statement
ivo_david_michelle 4:3040d0f9e8c6 84
ivo_david_michelle 18:a00d6b065c6b 85 Thread threadC(controller_thread);
ivo_david_michelle 4:3040d0f9e8c6 86 pc.printf("Entering control loop\n\r");
ivo_david_michelle 4:3040d0f9e8c6 87
ivo_david_michelle 4:3040d0f9e8c6 88 while (1) {
ivo_david_michelle 19:39c144ca2a2f 89 //myQuadcopter.readSensorValues();
ivo_david_michelle 18:a00d6b065c6b 90 //pc.printf("%lld: %f,%f,%f,%f\r\n", myQuadcopter.id, myQuadcopter.thrust, myQuadcopter.yaw, myQuadcopter.pitch, myQuadcopter.roll);
ivo_david_michelle 11:5c54826d23a7 91
ivo_david_michelle 15:90e07946186f 92 // myQuadcopter.controller();
ivo_david_michelle 13:291ba30c7806 93
ivo_david_michelle 17:96d0c72e413e 94 // wait(0.01);
ivo_david_michelle 1:b87e95907a18 95
ivo_david_michelle 4:3040d0f9e8c6 96 // Set duty cycle
ivo_david_michelle 18:a00d6b065c6b 97 // motors motorsPwm=myQuadcopter.getPwm();
ivo_david_michelle 5:f007542f1dab 98
ivo_david_michelle 18:a00d6b065c6b 99 //motor_2=motorsPwm.m2;
ivo_david_michelle 18:a00d6b065c6b 100 //motor_4=motorsPwm.m4;
ivo_david_michelle 15:90e07946186f 101
ivo_david_michelle 14:64b06476d943 102 //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 15:90e07946186f 103 // pc.printf("m1: %f m2: %f \n\r", motorsPwm.m2, motorsPwm.m4);
ivo_david_michelle 11:5c54826d23a7 104 //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 105 }
ivo_david_michelle 0:4c04e4fd1310 106 }