Low-level TSI test

#include "mbed.h"
#include "TSIHW.h"

TSI touch(0,0,0,2,2);
DigitalOut lr(PTB18);
DigitalOut lg(PTB19);
DigitalOut lb(PTD1);

uint16_t b1, b2, b3, b4, s1, s2, s3, s4;

void GetBase() {
    int t1, t2, t3, t4;
    char k;
    uint16_t t;
    
    t1 = 0;
    t2 = 0;
    t3 = 0;
    t4 = 0;
    for(k = 0; k < 100; k++) {
        t = 0;
        printf("Baseline 1\r\n"); // THIS PRINTF CAN BE REMOVED WITHOUT PROBLEMS
        touch.Start(5);
        while(!t) {
            t = touch.Read();
        }
        t1 = t1 + t;
        t = 0;
        printf("Baseline 2\r\n");
        touch.Start(13);
        while(!t) {
            t = touch.Read();
        }
        t2 = t2 + t;
        t = 0;
        printf("Baseline 3\r\n");
        touch.Start(15);
        while(!t) {
            t = touch.Read();
        }
        t3 = t3 + t;
        t = 0;
        printf("Baseline 4\r\n");
        touch.Start(14);
        while(!t) {
            t = touch.Read();
        }
        t4 = t4 + t;
    }
    b1 = t1 / 90;
    b2 = t2 / 90;
    b3 = t3 / 90;
    b4 = t4 / 90;
}

void Check() {
    s1 = 0;
    printf("Sensor 1\r\n"); // THIS PRINTF CAN BE REMOVED WITHOUT PROBLEMS
    touch.Start(5);
    while(!s1) {
        s1 = touch.Read();
    }
    s2 = 0;
    printf("Sensor 2\r\n");
    touch.Start(13);
    while(!s2) {
        s2 = touch.Read();
    }
    s3 = 0;
    printf("Sensor 3\r\n");
    touch.Start(15);
    while(!s3) {
        s3 = touch.Read();
    }
    s4 = 0;
    printf("Sensor 4\r\n");
    touch.Start(14);
    while(!s4) {
        s4 = touch.Read();
    }
}

int main() {
    lr = 1;
    lg = 1;
    lb = 1;
    
    touch.ActivateChannel(5); // OK button
    touch.ActivateChannel(13); // LEFT button
    touch.ActivateChannel(14); // RIGHT button
    touch.ActivateChannel(15); // CANCEL button
    
    GetBase();
        
    printf("BASELINES: %d, %d, %d, %d\r\n", b1, b2, b3, b4);
    while(1) {
        Check();
        printf("SIGNALS: %d, %d, %d, %d\r\n", s1, s2, s3, s4);
        if(s1 > b1) {
            lr = 0;
        } else {
            lr = 1;
        }
        
        if(s2 > b2) {
            lg = 0;
        } else {
            lg = 1;
        }
        
        if(s3 > b3) {
            lb = 0;
        } else {
            lb = 1;
        }
                
        wait(0.3);
    }
}