File content as of revision 0:d935ea6f5c25:

#include "initialize.h"
#include "Controller.h"
int main() {
    calibrateButton.rise(&calibrate);
    motorControl.attach(&setFlag, RATE);
    //[FIXME]calibratePowerrrr(); store max amplitude each emg signal
    run();
}

void run() {
    while(true){
        double r = signal.readValue(1); // returns filtered value
        double l = signal.readValue(2);
        double u = signal.readValue(3);
        double d = signal.readValue(4);
        
        // calculate r,l,u,d in a % of max 
        double percentageR = r/rMax;
        double percentageL = l/lMax;
        double percentageU = u/uMax;
        double percentageD = d/dMax;
        
        
        if (flag) {
            // ignore the weaker signal and motorValue is calculate here
            if ( percentageR > percentageL){ // horizontal speed
                motorValueHor = percentageR;
            } else {motorValueHor = -percentageL;}
            if (percentageU > percentageD) { // vertical speed 
                motorValueVer = percentageU;
            } else {motorValueVer = -percentageD;}
            
            // current pulses and prevpulses calculated
            x += (pulses.getDistanceHor() - prevX); // in pulses
            prevX = x;
            y += (pulses.getDistanceVer() - prevY);
            prevY = y;
            
            //edge detection: for now it will calibrate; implement function that allows movement for the other directions 
            if (x > x_max || x < 0) { 
                emergencyExit();
            }
            if (y > y_max || y < 0) { //
                emergencyExit(); 
            }
            
            // Quantize
            if (motorValueHor > maxHorVel){motorValueHor = maxHorVel;}
            if (motorValueHor < -maxHorVel){motorValueHor = -maxHorVel;}
            if (motorValueHor > 0 && motorValueHor < maxHorVel){motorValueHor = 0.5*maxHorVel;}
            if (motorValueHor < 0 && motorValueHor > -maxHorVel){motorValueHor = -0.5*maxHorVel;}
                
            if ( motorValueVer > maxVerVel){motorValueVer = 1;}
            if (motorValueVer < -1){motorValueVer = -1;}
            if (motorValueVer > 0 && motorValueVer < 1){motorValueVer = 0.5;}
            if (motorValueVer < 0 && motorValueVer > -1){motorValueVer = -0.5;}
            
            // Calculate desired rotational velocity from linear velocity
            
            // Calculate current rotational velocity
            
            // give the PID controller the setpoint and processvariable
            
            // compute PID
            
            // output to motor
           // motorOutput(toMotorValue(percentageR, percentageL), true); // [FIXME]- safety edges (if statement)
           // motorOutput(toMotorValue(percentageU, percentageD), false); // replace this with all the calculations and PID class stuff
           
           // output to hidscope
            flag = false; // only output to motor with RATE
        }
        
    }
}
void calibrate() {
    // GO-TO pos (0,0)
    // pulses can only be 0 or positive so reverse the direction until 0 pulses is reached
    // problem with the interrupt button, after calibration the program should re-start the run function instead of going further at the point it left
    // this will cause 1 iteration to be absolutely wrong -> consequences?
    return;
}

void motorOutput(double motorValue, bool horizontal) { // IMPLEMENT DEADZONE?
    if (motorValue > 1) { // safety
        motorValue = 1;
    }
    if (motorValue < -1) {
        motorValue = -1;
    }
    
    // deadzone here
    
    if (horizontal) {
        motor.outputHor(fabs(motorValue), (motorValue>0) );
    } else {
        motor.outputVer(fabs(motorValue), (motorValue>0));
    }
}

void setFlag() {
    flag = true;
}

void emergencyExit() {
    calibrate();
    run();
 }