File content as of revision 1:b9005f2aabf5:

#include "initialize.h"
#include "Controller.h"

int main() {
    calibrateButton.rise(&calibrate);
    motorControl.attach(&setFlag, RATE);
    initMotors();
    initControllers();
    calibratePower(); // 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) { // setting the flag here will cause the edge detection to be ignored if the flag is false for some reason
            // ignore the weaker signal,motorValue calculation, threshold set
            if ( percentageR > percentageL){ // horizontal speed request
                motorValueHor = percentageR;
                minHorThreshold = 0.2*rMax; // set the threshold
            } else {motorValueHor = -percentageL;
                  minHorThreshold = 0.2*lMax;
              }
            if (percentageU > percentageD) { // vertical speed request
                motorValueVer = percentageU;
                minVerThreshold = 0.2*uMax;
            } else {motorValueVer = -percentageD;
                  minVerThreshold = 0.2*uMax;
              }
            // current pulses and prevpulses calculated
            x += (pulses.getDistanceHor() - prevX); // in pulses
            prevX = x;
            y += (pulses.getDistanceVer() - prevY);
            prevY = y;
            
            //edge detection
            if (x > x_max && motorValueHor > 0) { // right should correspond to positive motorvalue
                motorValueHor = 0;
            }
            if (x < 0 && motorValueHor < 0) { // carefulwith 0 pulses, maybe implement margin
                motorValueHor = 0;
            }
            if (y > y_max && motorValueVer > 0) { // up should correspond to positive motorvalue
                motorValueVer = 0;
            }
            if (y < 0 && motorValueVer < 0) {
                motorValueVer =0;
            } 
            
            // Quantize the linear speed request
            // motorValue is a value [-1,1], currently only 0 or 1/-1
            if (motorValueHor > maxHorValue){motorValueHor = maxHorValue;} // safety
            if (motorValueHor < -maxHorValue){motorValueHor = -maxHorValue;}
            if (motorValueHor > minHorThreshold && motorValueHor < maxHorValue){motorValueHor = maxHorValue;} // if threshold is met pwm is 100%, maybe quantize here
            if (motorValueHor < -minHorThreshold && motorValueHor > -maxHorValue){motorValueHor = -maxHorValue;}
                
            if (motorValueVer > maxVerValue){motorValueVer = maxVerValue;}
            if (motorValueVer < -maxVerValue){motorValueVer = -maxVerValue;}
            if (motorValueVer > minVerThreshold && motorValueVer < maxVerValue){motorValueVer = maxVerValue;}
            if (motorValueVer < -minVerThreshold && motorValueVer > -maxVerValue){motorValueVer = -maxVerValue;}
            
            //current position of pen in degrees
            currentXangle = 360.0f*(pulses.getDistanceHor()/8400);
            currentYangle = 360.0f*(pulses.getDistanceVer()/8400); 
            
            // current position of pen in cm
            currentX = (360.0f/(2*PI))*(pulses.getDistanceHor()/8400)*(RmHor/R1); // radiants*revolutions*gearratio is x pos in rad IS RAD EQUAL TO CM?????????
            currentY= (360.0f/(2*PI))*(pulses.getDistanceVer()/8400); // radiants*revolutions 1:1 ratio??
            
            // desired position of pen in cm
            toX = currentX + motorValueHor*maxSpeed*RATE;
            toY = currentY + motorValueVer*maxSpeed*RATE; 
            
            // desired angle of pen in degrees
            toangleX = ((toX-currentX)*(R1/RmHor))*(360.0f/(2*PI)); 
            toangleY = (toX-currentX)*(360.0f/(2*PI)); 
            
            //requested velocities in degrees/sec
            Xvelrequest = (toangleX - currentXangle) / RATE; 
            Yvelrequest = (toangleY - currentYangle) / RATE;
            
            // set setpoint of PID controller here with velrequest, what are the input limits???
            
            // Calculate current rotational velocity
            
            // give the PID controller the processvariable
            
            // compute PID
            
            // output to motor
           
           // output to hidscope
            flag = false; // only output to motor with RATE
        }
        
    }
}

void setFlag() {
    flag = true;
}

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

void calibrate() {
    while ((pulses.getDistanceHor() > margin) && (pulses.getDistanceVer() > margin)){
        if (pulses.getDistanceHor() > margin){
        // go direction LEFT
        }
        else { //motormaghor 0    
        }
        if (pulses.getDistanceVer() > margin){
        // go direction DOWN
        }
        else { //motormagver 0}     
        }
        // 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?
    }
}   
 
void calibratePower(){
    while((clock()/CLOCKS_PER_SEC) < 5){ // FIXME
        // do this for 5 sec
        double rCal = signal.readValue(1); 
        if ( rCal > rMax){
            rMax = rCal;
        }
        double lCal = signal.readValue(2);
        if ( lCal > lMax){
            lMax = lCal;
        }
        double uCal = signal.readValue(3);
        if ( uCal > uMax){
            uMax = uCal;
        }
        double dCal = signal.readValue(4);
        if ( dCal > dMax){
            dMax = dCal;
        }
    }
    return;
}