File content as of revision 29:1c5254b27851:

#include "controller.h"
#include "BiQuad.h"

// ====== Hardware stuff ======

/* The robot controller */
RobotController robotController;

/* The EMG inputs */
AnalogIn emg1(A0);
AnalogIn emg2(A1);

/* Used in calibration */
DigitalIn calibrating(SW2);
/* Used to start calibrating */
InterruptIn calibrateButton(SW3);

/* LEDs
 RED FLICKERING     --> Ready to calibrate (press button SW3 to start)
 GREEN FlICKERING   --> Calibration success
 BLUE               --> Busy calibrating
*/
DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);
DigitalOut led_blue(LED_BLUE);

/* ABORT! ABORT! */
InterruptIn killButton(D3);

/*For debuggin purposes*/
Serial pc(USBTX, USBRX);

//====== Constants =====

enum RobotCommand{NOTHING, UP, DOWN, FORWARD, BACKWARD};
enum ProgramState{CALIBRATING, UPDOWN, FORBACK};

const float sampleFrequency = 500;
const float sampleTime = 1.0f/sampleFrequency;


//====== Program Variables ======

volatile ProgramState progState;
volatile RobotCommand robotCommand;

/*The 'main' ticker which samples our emg signals at the control state*/
Ticker ticker;
/*The ticker used for calibration*/
Ticker sampler;

const float sample_frequency = 500.0f; //Hz
const float Ts = 1.0f / sample_frequency;

//Counts how many signals have passed. Resets at 50. See processEMG.
volatile int count = 0;

/*Function used to send data to the motor*/
void (*motorFunc)(bool, bool);



/* EMG BiQuadChain 1 */
BiQuadChain bqc1;
//Notch iir filter.
//Notch:    50 +- 2 Hz
BiQuad bq1(9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );


/* EMG BiQuadChain 2 */
BiQuadChain bqc2;
//Notch iir filter.
//Notch:    50 +- 2 Hz
BiQuad bq2( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );


// Arrays used in the calibrationi phase
// Values in these arrays contain samples that are already notched and rectified.
const int calibrateNumEmgCache = 100;
float calibrateEmgCache1[calibrateNumEmgCache]; //sorted from new to old;
float calibrateEmgCache2[calibrateNumEmgCache]; //sorted from new to old;

// Arrays used to calculate the moving average
// Values in these arrays contain samples that are already notched and rectified.
const int numEmgCache = 50;
float emgCache1[numEmgCache]; //sorted from new to old;
float emgCache2[numEmgCache]; //sorted from new to old;


// Thresholds for the decisioin. by default 0.2, 
// The values are changed during calibration.
volatile float threshold1 = 0.2;
volatile float threshold2 = 0.2;

// The last 50 signals that have been dititalised.
// Only contains ones and zeros.
int decided1[numEmgCache];
int decided2[numEmgCache];


//====== Functions ======

// Helper Functions

void resetLeds() {
    led_red = true;
    led_green = true;
    led_blue = true;    
}    

// Rotates the array one position, replacing the first value with the new value
void addFirst(float newValue, float array[], int size) {
    for (int i = size - 2; i >= 0; i--) {
        array[i+1] = array[i];
    }
    array[0] = newValue;
}

// Rotates the array one position, replacing the first value with the new value
void addFirst(int newValue, int array[], int size) {
    for (int i = size - 2; i >= 0; i--) {
        array[i+1] = array[i];
    }
    array[0] = newValue;
}
 

float sum(float array[], int size) {
    float sum = 0;
    for (int i = 0; i < size; i++) {
        sum += array[i];
    }
    return sum;
}

float mean(float array[], int size) {
    return sum(array, size) / size;
}

// 'Digitize' an analog value by comparing to a threshold
int decide(float value, float threshold) {
    return value < threshold ? 0 : 1;
}

//shifts the array by adding the new emg value up front.
//returns the new calculated average
float movingAverage(float newValue, float array[], int size) {
    float sum = 0;
    for (int i = size - 2; i >= 0; i--) {
        array[i+1] = array[i];
        sum += array[i];
    }
    array[0] = newValue;
    sum += newValue;
    return sum / size;
}


float rectifier(float value) {
    return fabs(value - 0.5f)*2.0f;
}

void sendToMotor(void (*func)(bool, bool), bool arg1, bool arg2) {
    func(arg1, arg2);
} 


// ====== Functions used for calibrations =====

void sample() {
    float emgOne = emg1.read();
    float notch1 = bqc1.step( emgOne );  
    
    float emgTwo = emg2.read();
    float notch2 = bqc2.step( emgTwo );  
    
    float rect1 = rectifier(notch1);
    float rect2 = rectifier(notch2);
    
    float filtered1 = movingAverage( rect1, calibrateEmgCache1, calibrateNumEmgCache);
    float filtered2 = movingAverage( rect2, calibrateEmgCache2, calibrateNumEmgCache);    
}

void calibrate() {
    while(calibrating) {
        led_red = false;
        wait(0.5);    
        led_red = true;
        wait(0.5);    
    }
    
    // Button pressed for rest measurement
    led_red = true;
    sampler.attach(&sample, Ts);
    led_blue = false;
    wait(10);
    // 10 seconds sampled
    led_blue = true;
    sampler.detach();
    float restAvg1 = mean(calibrateEmgCache1, calibrateNumEmgCache);
    float restAvg2 = mean(calibrateEmgCache2, calibrateNumEmgCache);
    
    int i =0;
    while(i<3) {
        led_green = false;
        wait(0.5);    
        led_green = true;
        wait(0.5);    
        i++;
    }
    led_green = true;
    
    while(calibrating) {
        led_red = false;
        wait(0.5);    
        led_red = true;
        wait(0.5);    
    }
    // Button pressed for contracted measurement    
    led_red = true;
    sampler.attach(&sample, Ts);
    led_blue = false;
    wait(10);
    
    // 10 seconds sampled
    led_blue = true;
    sampler.detach();
    
    i =0;
    while(i<3) {
        led_green = false;
        wait(0.5);    
        led_green = true;
        wait(0.5);    
        i++;
    }
    
    float contAvg1 = mean(calibrateEmgCache1, calibrateNumEmgCache);
    float contAvg2 = mean(calibrateEmgCache2, calibrateNumEmgCache);
    
    threshold1 = (contAvg1 + restAvg1)/2;
    threshold2 = (contAvg2 + restAvg2)/2;    
    //pc.printf("threshold1: %f\tthreshold2:%f\n\r", threshold1, threshold2);

}

// ===== The main functions called by our main ticker ======

/**
 * processEMG function
 * This function is called by our ticker.
 * This function measures emg and applies the filters.
 * out of 50 emg values, a 1 or 0 is chosen depending on which occurs the most
 * then that value is used in the motor func.
 */        
void processEMG() {
    //read emg
    float emgOne = emg1.read();
    float emgTwo = emg2.read();
    
    //apply notch filter
    float notch1 = bqc1.step( emgOne );      
    float notch2 = bqc2.step( emgTwo );  
    
    //then apply rectifier   
    float rect1 = rectifier(notch1);
    float rect2 = rectifier(notch2);
    
    //then apply moving average
    float filtered1 = movingAverage( rect1, emgCache1, numEmgCache);
    float filtered2 = movingAverage( rect2, emgCache2, numEmgCache);

    //decide on wether the signal was strong enough (1) or too weak (0)
    int decide1 = decide(mean(emgCache1, numEmgCache ), threshold1);
    int decide2 = decide(mean(emgCache2, numEmgCache ), threshold2);
    
    //add boolean value in front of the boolean arrays
    addFirst(decide1, decided1, numEmgCache);
    addFirst(decide2, decided2, numEmgCache);   

    //after 50 calls of this function, control the motor.
    if (count >= 49) {
        int counter1=0;
        int counter2=0;
        for(int i = 0; i < numEmgCache; ++i){
            if(decided1[i] == 0)
                ++counter1;
            if(decided2[i] == 0)
                ++counter2;        
        }
        // avgDecide1 = true if the decided1 array contains mostly ones, otherwise false.
        int avgDecide1 = counter1 > std::ceil(numEmgCache/2.0) ? 0: 1;
        // avgDecide2 = true if the decided2 array contains mostly ones, otherwise false.
        int avgDecide2 = counter2 > std::ceil(numEmgCache/2.0) ? 0: 1;
        
        // Use these values to contorl the motor.
        sendToMotor(motorFunc,avgDecide1, avgDecide2);
   
        //reset function call count to 0
        count =0;
    } else {
        count++;   
    }    
}

void getXandY(float &x, float &y) {
    float lower = robotController.getLowerArmLength();
    float upper = robotController.getUpperArmLength();
    getRollerPositionForArmLengths(upper, lower, x, y);
}

//wheter the user can still execute a command
Timeout commandDelayer;
volatile bool canCommand = true;
void enableCommand() {
    canCommand = true;
}

Timeout commandSequencer;
void doPaintUp() {
    robotController.paintUp();
}
void doPaintDown() {
    robotController.paintDown();
}

float xmin = 25;
float xmax = 40;
float ymin = 20;
float ymax = 50;

/* executes the robot command */
void processCommand(RobotCommand cmd) {
    if (!canCommand || cmd == robotCommand) return;
    
    robotCommand = cmd;
    
    //user can only switch commands once every 2 seconds
    canCommand = false;
    commandDelayer.attach(&enableCommand, 2.0f);
    
    switch (robotCommand) {
        float x;
        float y;
        case UP:
            robotController.goToBottom();
            commandSequencer.attach(&doPaintUp, 3.0f);
            break;
        case DOWN:
            robotController.goToTop();
            commandSequencer.attach(&doPaintDown, 3.0f);
            break;
        case FORWARD:
            getXandY(x, y);
            robotController.moveTo(xmax, h + 0.5*d);
            break;
        case BACKWARD:
            getXandY(x, y);
            robotController.moveTo(xmin, h + 0.5*d);
            break;
        case NOTHING:
            
            break;
    }
}

//some little utils used by the function below
Timeout switchBlocker;
volatile bool justSwitched;
void unblockSwitch() {
    justSwitched = false;
}

//tries to switch the state.
//returns true if it was successfull
//or false if we couldn't switch.
bool switchState() {
    if (justSwitched) return false;
    justSwitched = true;
    switch(progState) {
        case UPDOWN:
            progState = FORBACK;
            break;
        case FORBACK:
            progState = UPDOWN;
            break;   
    }
    pc.printf("\r\n\n\n1 and 1, switch state to %s.\r\n\n\n", progState == UPDOWN ? "UPDOWN" : "FORBACK");
     //we can only switch once per 2 seconds
    switchBlocker.attach(&unblockSwitch, 2.0f);
    return true;
}

/* Translates our two digital signals to robot commands */
void onSignal(bool emg1, bool emg2) {
    RobotCommand command = NOTHING;
    if (emg1 && emg2) {
        switchState();
        processCommand(command);
        return;
    }
    switch(progState) {
        case UPDOWN:
            if (emg1) {
                command = UP;
                pc.printf("1 and 0, command = UP\r\n");
            } else if (emg2) {
                command = DOWN;
                pc.printf("0 and 1, command = DOWN\r\n");
            }
            break;
        case FORBACK:
            if (emg1) {
                command = FORWARD;
                pc.printf("1 and 0, command = FORWARD\r\n");
            } else if (emg2) {
                command = BACKWARD;
                pc.printf("0 and 1, command = BACKWARD\r\n");
            }
            break;
    }
    
    if (!emg1 && !emg2) {
        pc.printf("0 and 0\r\n");
    }
    
    //execute the command
    processCommand(command);
}

void consumeBools(bool x, bool y) {
    onSignal(x, y);
}


// ====== The entry point of our programme ======


int main() //TODO this will become the actual main!
{
    pc.baud(115200);
    killButton.fall(robotController.getRobot(), &Robot::kill);
    
    // initial state
    resetLeds();
    progState = CALIBRATING;
    robotCommand = NOTHING;
    
    // initialize notch filters
    bqc1.add( &bq1 );
    bqc2.add( &bq2 );
    
    // Attach cablitrate function to the button to be able to calibrate again
    // If the user desires so
    calibrateButton.fall(&calibrate);
    
    // The function that takes our ditised signals and controls the robot
    motorFunc = &consumeBools;
    

    // call the calibrating function once at the start
    // this function blocks until the calibration phase is over
    calibrate();
    
    // After calibration the program state is UPDOWN
    progState = UPDOWN;
    
    
    // 500 HZ Ticker
    ticker.attach(&processEMG, Ts);
    
    while (true);
}