Diff: test_main.cpp

Revision:

19:a82b55a15457

Parent:

18:1c9dc6caab9d

Parent:

16:66326e4a40b1

Child:

21:2aed81380bc3

--- a/test_main.cpp	Thu Nov 03 16:29:31 2016 +0000
+++ b/test_main.cpp	Thu Nov 03 16:31:28 2016 +0000
@@ -1,60 +1,403 @@
-//#include "mbed.h"
-//#include "robot.h"
-//
-//// ====== Hardware stuff ======
-//
-//Robot robot;
-//
-//AnalogIn emg1(A0);
-//AnalogIn emg2(A1);
-//
-//DigitalOut red(LED_RED);
-//DigitalOut green(LED_GREEN);
-//DigitalOut blue(LED_BLUE);
-//
-////====== 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 ======
-//
-//ProgramState progState;
-//RobotCommand robotCommand;
-//
-//
-////====== Functions ======
-//
-//
-//void calibrate(void) {
-//    //Calibrate function -- blocking.
-//    //Calculates and sets both emg1threshold and emg2 threshold
-//    
-//    
-//    
-//}
-//
-//void run() {
-//    //Run function -- blocking
-//    //runs the calibrated robot
-//
-//}
-//
-//
-////int main() {
-////    progState = CALIBRATING;
-////    
-////    calibrate();
-////    
-////    progState = UPDOWN;
-////    robotCommand = NOTHING;
-////    
-////    run();
-////    
-////    while(true);
-////    return 0;
-////}
\ No newline at end of file
+#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);
+
+/*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;
+volatile int count = 0; //how many signals have passed. resets at 50.
+
+/*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;
+}
+
+// Functions used for filtering
+
+//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 ======
+        
+void processEMG() {
+    float emgOne = emg1.read();
+    float emgTwo = emg2.read();
+    float notch1 = bqc1.step( emgOne );      
+    float notch2 = bqc2.step( emgTwo );  
+       
+    float rect1 = rectifier(notch1);
+    float rect2 = rectifier(notch2);
+    
+    float filtered1 = movingAverage( rect1, emgCache1, numEmgCache);
+    float filtered2 = movingAverage( rect2, emgCache2, numEmgCache);
+
+    int decide1 = decide(mean(emgCache1, numEmgCache ), threshold1);
+    int decide2 = decide(mean(emgCache2, numEmgCache ), threshold2);
+    addFirst(decide1, decided1, numEmgCache);
+    addFirst(decide2, decided2, numEmgCache);   
+
+    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;        
+        }
+        int avgDecide1 = counter1 > std::ceil(numEmgCache/2.0) ? 0: 1;
+        int avgDecide2 = counter2 > std::ceil(numEmgCache/2.0) ? 0: 1;
+        sendToMotor(motorFunc,avgDecide1, avgDecide2);
+   
+        count =0;
+    } else {
+        count++;   
+    }    
+}
+
+/* executes the robot command */
+void processCommand(RobotCommand cmd) {
+    if (cmd == robotCommand) return;
+    
+    robotCommand = cmd;
+    
+    switch (robotCommand) {
+        case UP:
+            robotController.paintUp();
+            break;
+        case DOWN:
+            robotController.paintDown();
+            break;
+        case FORWARD:
+            //TODO
+            break;
+        case BACKWARD:
+            //TODO
+            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;
+    }
+     //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;
+            } else if (emg2) {
+                command = DOWN;
+            }
+            break;
+        case FORBACK:
+            if (emg1) {
+                command = FORWARD;
+            } else if (emg2) {
+                command = BACKWARD;
+            }
+            break;
+    }
+    
+    //execute the command
+    processCommand(command);
+}
+
+void consumeBools(bool x, bool y) {
+    //pc.printf("%d\t%d\r\n", x, y);
+    onSignal(x, y);
+}
+
+
+// ====== The entry point of our programme ======
+
+int test_main() //TODO this will become the actual main!
+{
+    //pc.baud(115200);
+    
+    // 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);
+}>>>>>>> other