File content as of revision 29:88c5bf096714:

#include "mbed.h"
#include "TSISensor.h"
#include "esc.h"
#include "PID.h"

Serial pc(USBTX, USBRX); // tx, rx
DigitalOut myled(LED1);

#define PIDTEST 0

void pidTest() {
    PID pid(1.5,0.1,1.20,500.0);
    float currAngle= 0;
    float targetAngle = 45;
    float correction;

    pc.printf("Hello World!\r\n");
    while(1) {
        myled = 1;
        wait(0.2);
        myled = 0;
        wait(0.2);
        
        correction = pid.correct(currAngle, targetAngle, 0.1);
        pc.printf("curr = %f, target = %f, correct = %f\r\n", currAngle, targetAngle, correction);
        currAngle = correction;
        wait(0.1);
    }
}

ESC esc1(PTD4);
ESC esc2(PTA12);
ESC esc3(PTA4);
ESC esc4(PTA5);

#define STEP 1

void stepMotorDemo() {
    printf("Initialized\r\n");
    char c;
    int var = 0;

    while(1) {
        c = pc.getc();
        
        if (c == 'u') {
            if (var < 100) {
                var++;
            }
            if (esc1.setThrottle(var) && esc2.setThrottle(var) && esc3.setThrottle(var) && esc4.setThrottle(var)) {
                printf("%i\r\n", var);
            }
        }
        else if (c == 'd') {
            if (var > 0) {
                var--;
            }
            if (esc1.setThrottle(var) && esc2.setThrottle(var) && esc3.setThrottle(var) && esc4.setThrottle(var)) {
                printf("%i\r\n", var);
            }
        }
        else if (c == 'r') {
            var = 0;
            if (esc1.setThrottle(var) && esc2.setThrottle(var) && esc3.setThrottle(var) && esc4.setThrottle(var)) {
                printf("%i\r\n", var);                
            }
        }
        
        esc1.pulse();
        esc2.pulse();
        esc3.pulse();
        esc4.pulse();
        wait_ms(20);
    }
}

#define TOUCH 2
int touchmodeDemo() {

    /* Electronic Speed Control */
    ESC esc1(PTD4);
    ESC esc2(PTC9);
    ESC esc3(PTC8);
    ESC esc4(PTA5);

    int throttleLevel = 0;


    /* Mode LEDs */
    PwmOut led1(LED_BLUE);
    PwmOut led2(LED_GREEN);
    PwmOut led3(LED_RED);

    led1 = 1;
    led2 = 1;
    led3 = 1;

    /* Capacitive Touch Sensor */
    TSISensor tsi;

    float percent=0;

    while (1) 
    {   
        if (tsi.readPercentage() > 0) {
            percent = tsi.readPercentage();
        }
        
        // LED=1,2,3 : blue=001, green=101, red=110
        led1 = (percent <= 0.33)?0:1;
        led2 = (percent > 0.66 && percent <= 1)?1:0;
        led3 = (percent > 0.66 && percent <= 1)?0:1;

        if (percent <= 0.33) {
            throttleLevel = 0;
            esc1.setThrottle(throttleLevel); esc2.setThrottle(throttleLevel); esc3.setThrottle(throttleLevel); esc4.setThrottle(throttleLevel);
        }
        else if (percent > 0.33 && percent <= 0.66) {
            throttleLevel = 15;
            esc1.setThrottle(throttleLevel); esc2.setThrottle(throttleLevel); esc3.setThrottle(throttleLevel); esc4.setThrottle(throttleLevel);
        }
        else if (percent > 0.66) {
            throttleLevel = 30;
            esc1.setThrottle(throttleLevel); esc2.setThrottle(throttleLevel); esc3.setThrottle(throttleLevel); esc4.setThrottle(throttleLevel);
        }
        esc1.pulse();
        esc2.pulse();
        esc3.pulse();
        esc4.pulse();
        wait_ms(20);
    }
}

#define MAIN STEP
int main() {
#if MAIN == PID;
    pidTest();
#elif MAIN == STEP;
    stepMotorDemo();
#elif MAIN == TOUCH
    touchmodeDemo;
#endif
}