![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
main.cpp
- Committer:
- ivo_david_michelle
- Date:
- 2016-04-10
- Revision:
- 20:efa15ed008b4
- Parent:
- 19:39c144ca2a2f
- Child:
- 21:336faf452989
File content as of revision 20:efa15ed008b4:
#include "mbed.h" #include "rtos.h" #define _MBED_ //#include "controller.h" #include "sensor.h" #include "quadcopter.h" Serial pc(USBTX, USBRX); MRF24J40 mrf(p11, p12, p13, p14, p21); Quadcopter myQuadcopter(&pc, &mrf); // instantiate Quadcopter object // pwm outputs for the 4 motors (motor 1 points into x direction, 2 into y direction, 3 -x direction, -y direction PwmOut motor_1(p23); PwmOut motor_2(p24); PwmOut motor_3(p25); PwmOut motor_4(p26); void controller_thread(void const *args) { while(true) { //myQuadcopter.readSensorValues(); myQuadcopter.controller(); motors motorsPwm=myQuadcopter.getPwm(); motor_2=0.5;//motorsPwm.m2; motor_4=motorsPwm.m4; pc.printf("m1: %f m2: %f \n\r", motorsPwm.m2, motorsPwm.m4); Thread::wait(10); } } void rc_thread(void const *args) { while(true) { myQuadcopter.readRc(); Thread::wait(50); // wait for some number of miliseconds } } int main() { //Thread thread(controller_thread); // get desired values from joystick (to be implemented) // myQuadcopter.setDes(...) motor_1.period(0.002); // motor requires a 2ms period motor_2.period(0.002); // motor requires a 2ms period motor_3.period(0.002); // motor requires a 2ms period motor_4.period(0.002); // motor requires a 2ms period // startup procedure pc.printf("Type 's' to start up Motors, or anything else to abort.\n\r"); char a= pc.getc(); if (a!='s') { pc.printf("Aborting"); return 0; }; // Duty cycle at beginning must be 50% pc.printf("Starting up ESCs\n\r"); motor_1 = 0.5; motor_2 = 0.5; motor_3 = 0.5; motor_4 = 0.5; pc.printf("Type 'c' to enter control loop, or anything else to abort.\n\r"); char b = pc.getc(); if (b!='c') { pc.printf("Aborting"); return 0; }; Thread threadR(rc_thread); //check if remote controll is working with if statement Thread threadC(controller_thread); pc.printf("Entering control loop\n\r"); while (1) { //myQuadcopter.readSensorValues(); //pc.printf("%lld: %f,%f,%f,%f\r\n", myQuadcopter.id, myQuadcopter.thrust, myQuadcopter.yaw, myQuadcopter.pitch, myQuadcopter.roll); // myQuadcopter.controller(); // wait(0.01); // Set duty cycle // motors motorsPwm=myQuadcopter.getPwm(); //motor_2=motorsPwm.m2; //motor_4=motorsPwm.m4; //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r", F, M_x, M_y, M_z); // pc.printf("m1: %f m2: %f \n\r", motorsPwm.m2, motorsPwm.m4); //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); } }