Derek McLean / Mbed 2 deprecated uwb-quadcopter

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed ESC SR04 TSI

main.cpp@29:88c5bf096714, 2013-06-09 (annotated)

Committer:
dereklmc
Date:
Sun Jun 09 00:29:43 2013 +0000
Revision:
29:88c5bf096714
Parent:
11:82e6ed0385e0 
Removed gy-80 library.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dereklmc 0:dd5f0f95e448 1 #include "mbed.h"
dereklmc 29:88c5bf096714 2 #include "TSISensor.h"
dereklmc 29:88c5bf096714 3 #include "esc.h"
dereklmc 11:82e6ed0385e0 4 #include "PID.h"
dereklmc 11:82e6ed0385e0 5
dereklmc 11:82e6ed0385e0 6 Serial pc(USBTX, USBRX); // tx, rx
dereklmc 0:dd5f0f95e448 7 DigitalOut myled(LED1);
dereklmc 0:dd5f0f95e448 8
dereklmc 29:88c5bf096714 9 #define PIDTEST 0
dereklmc 29:88c5bf096714 10
dereklmc 29:88c5bf096714 11 void pidTest() {
dereklmc 29:88c5bf096714 12 PID pid(1.5,0.1,1.20,500.0);
dereklmc 11:82e6ed0385e0 13 float currAngle= 0;
dereklmc 11:82e6ed0385e0 14 float targetAngle = 45;
dereklmc 11:82e6ed0385e0 15 float correction;
dereklmc 11:82e6ed0385e0 16
dereklmc 11:82e6ed0385e0 17 pc.printf("Hello World!\r\n");
dereklmc 0:dd5f0f95e448 18 while(1) {
dereklmc 0:dd5f0f95e448 19 myled = 1;
dereklmc 0:dd5f0f95e448 20 wait(0.2);
dereklmc 0:dd5f0f95e448 21 myled = 0;
dereklmc 0:dd5f0f95e448 22 wait(0.2);
dereklmc 11:82e6ed0385e0 23
dereklmc 11:82e6ed0385e0 24 correction = pid.correct(currAngle, targetAngle, 0.1);
dereklmc 11:82e6ed0385e0 25 pc.printf("curr = %f, target = %f, correct = %f\r\n", currAngle, targetAngle, correction);
dereklmc 11:82e6ed0385e0 26 currAngle = correction;
dereklmc 11:82e6ed0385e0 27 wait(0.1);
dereklmc 0:dd5f0f95e448 28 }
dereklmc 0:dd5f0f95e448 29 }
dereklmc 29:88c5bf096714 30
dereklmc 29:88c5bf096714 31 ESC esc1(PTD4);
dereklmc 29:88c5bf096714 32 ESC esc2(PTA12);
dereklmc 29:88c5bf096714 33 ESC esc3(PTA4);
dereklmc 29:88c5bf096714 34 ESC esc4(PTA5);
dereklmc 29:88c5bf096714 35
dereklmc 29:88c5bf096714 36 #define STEP 1
dereklmc 29:88c5bf096714 37
dereklmc 29:88c5bf096714 38 void stepMotorDemo() {
dereklmc 29:88c5bf096714 39 printf("Initialized\r\n");
dereklmc 29:88c5bf096714 40 char c;
dereklmc 29:88c5bf096714 41 int var = 0;
dereklmc 29:88c5bf096714 42
dereklmc 29:88c5bf096714 43 while(1) {
dereklmc 29:88c5bf096714 44 c = pc.getc();
dereklmc 29:88c5bf096714 45
dereklmc 29:88c5bf096714 46 if (c == 'u') {
dereklmc 29:88c5bf096714 47 if (var < 100) {
dereklmc 29:88c5bf096714 48 var++;
dereklmc 29:88c5bf096714 49 }
dereklmc 29:88c5bf096714 50 if (esc1.setThrottle(var) && esc2.setThrottle(var) && esc3.setThrottle(var) && esc4.setThrottle(var)) {
dereklmc 29:88c5bf096714 51 printf("%i\r\n", var);
dereklmc 29:88c5bf096714 52 }
dereklmc 29:88c5bf096714 53 }
dereklmc 29:88c5bf096714 54 else if (c == 'd') {
dereklmc 29:88c5bf096714 55 if (var > 0) {
dereklmc 29:88c5bf096714 56 var--;
dereklmc 29:88c5bf096714 57 }
dereklmc 29:88c5bf096714 58 if (esc1.setThrottle(var) && esc2.setThrottle(var) && esc3.setThrottle(var) && esc4.setThrottle(var)) {
dereklmc 29:88c5bf096714 59 printf("%i\r\n", var);
dereklmc 29:88c5bf096714 60 }
dereklmc 29:88c5bf096714 61 }
dereklmc 29:88c5bf096714 62 else if (c == 'r') {
dereklmc 29:88c5bf096714 63 var = 0;
dereklmc 29:88c5bf096714 64 if (esc1.setThrottle(var) && esc2.setThrottle(var) && esc3.setThrottle(var) && esc4.setThrottle(var)) {
dereklmc 29:88c5bf096714 65 printf("%i\r\n", var);
dereklmc 29:88c5bf096714 66 }
dereklmc 29:88c5bf096714 67 }
dereklmc 29:88c5bf096714 68
dereklmc 29:88c5bf096714 69 esc1.pulse();
dereklmc 29:88c5bf096714 70 esc2.pulse();
dereklmc 29:88c5bf096714 71 esc3.pulse();
dereklmc 29:88c5bf096714 72 esc4.pulse();
dereklmc 29:88c5bf096714 73 wait_ms(20);
dereklmc 29:88c5bf096714 74 }
dereklmc 29:88c5bf096714 75 }
dereklmc 29:88c5bf096714 76
dereklmc 29:88c5bf096714 77 #define TOUCH 2
dereklmc 29:88c5bf096714 78 int touchmodeDemo() {
dereklmc 29:88c5bf096714 79
dereklmc 29:88c5bf096714 80 /* Electronic Speed Control */
dereklmc 29:88c5bf096714 81 ESC esc1(PTD4);
dereklmc 29:88c5bf096714 82 ESC esc2(PTC9);
dereklmc 29:88c5bf096714 83 ESC esc3(PTC8);
dereklmc 29:88c5bf096714 84 ESC esc4(PTA5);
dereklmc 29:88c5bf096714 85
dereklmc 29:88c5bf096714 86 int throttleLevel = 0;
dereklmc 29:88c5bf096714 87
dereklmc 29:88c5bf096714 88
dereklmc 29:88c5bf096714 89 /* Mode LEDs */
dereklmc 29:88c5bf096714 90 PwmOut led1(LED_BLUE);
dereklmc 29:88c5bf096714 91 PwmOut led2(LED_GREEN);
dereklmc 29:88c5bf096714 92 PwmOut led3(LED_RED);
dereklmc 29:88c5bf096714 93
dereklmc 29:88c5bf096714 94 led1 = 1;
dereklmc 29:88c5bf096714 95 led2 = 1;
dereklmc 29:88c5bf096714 96 led3 = 1;
dereklmc 29:88c5bf096714 97
dereklmc 29:88c5bf096714 98 /* Capacitive Touch Sensor */
dereklmc 29:88c5bf096714 99 TSISensor tsi;
dereklmc 29:88c5bf096714 100
dereklmc 29:88c5bf096714 101 float percent=0;
dereklmc 29:88c5bf096714 102
dereklmc 29:88c5bf096714 103 while (1)
dereklmc 29:88c5bf096714 104 {
dereklmc 29:88c5bf096714 105 if (tsi.readPercentage() > 0) {
dereklmc 29:88c5bf096714 106 percent = tsi.readPercentage();
dereklmc 29:88c5bf096714 107 }
dereklmc 29:88c5bf096714 108
dereklmc 29:88c5bf096714 109 // LED=1,2,3 : blue=001, green=101, red=110
dereklmc 29:88c5bf096714 110 led1 = (percent <= 0.33)?0:1;
dereklmc 29:88c5bf096714 111 led2 = (percent > 0.66 && percent <= 1)?1:0;
dereklmc 29:88c5bf096714 112 led3 = (percent > 0.66 && percent <= 1)?0:1;
dereklmc 29:88c5bf096714 113
dereklmc 29:88c5bf096714 114 if (percent <= 0.33) {
dereklmc 29:88c5bf096714 115 throttleLevel = 0;
dereklmc 29:88c5bf096714 116 esc1.setThrottle(throttleLevel); esc2.setThrottle(throttleLevel); esc3.setThrottle(throttleLevel); esc4.setThrottle(throttleLevel);
dereklmc 29:88c5bf096714 117 }
dereklmc 29:88c5bf096714 118 else if (percent > 0.33 && percent <= 0.66) {
dereklmc 29:88c5bf096714 119 throttleLevel = 15;
dereklmc 29:88c5bf096714 120 esc1.setThrottle(throttleLevel); esc2.setThrottle(throttleLevel); esc3.setThrottle(throttleLevel); esc4.setThrottle(throttleLevel);
dereklmc 29:88c5bf096714 121 }
dereklmc 29:88c5bf096714 122 else if (percent > 0.66) {
dereklmc 29:88c5bf096714 123 throttleLevel = 30;
dereklmc 29:88c5bf096714 124 esc1.setThrottle(throttleLevel); esc2.setThrottle(throttleLevel); esc3.setThrottle(throttleLevel); esc4.setThrottle(throttleLevel);
dereklmc 29:88c5bf096714 125 }
dereklmc 29:88c5bf096714 126 esc1.pulse();
dereklmc 29:88c5bf096714 127 esc2.pulse();
dereklmc 29:88c5bf096714 128 esc3.pulse();
dereklmc 29:88c5bf096714 129 esc4.pulse();
dereklmc 29:88c5bf096714 130 wait_ms(20);
dereklmc 29:88c5bf096714 131 }
dereklmc 29:88c5bf096714 132 }
dereklmc 29:88c5bf096714 133
dereklmc 29:88c5bf096714 134 #define MAIN STEP
dereklmc 29:88c5bf096714 135 int main() {
dereklmc 29:88c5bf096714 136 #if MAIN == PID;
dereklmc 29:88c5bf096714 137 pidTest();
dereklmc 29:88c5bf096714 138 #elif MAIN == STEP;
dereklmc 29:88c5bf096714 139 stepMotorDemo();
dereklmc 29:88c5bf096714 140 #elif MAIN == TOUCH
dereklmc 29:88c5bf096714 141 touchmodeDemo;
dereklmc 29:88c5bf096714 142 #endif
dereklmc 29:88c5bf096714 143 }