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?
User | Revision | Line number | New 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 | } |