File content as of revision 2:6b913f93bc0b:

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

int main() {
    calibrateButton.rise(&calibrate);
    motorControl.attach(&setFlag, RATE); // rate =0.001
    initMotors();
    initControllers();
    calibrate(); // calibrate position
    calibratePower(); // store max amplitude each emg signal
    run();
}

void run() {
    while(true){
        if(flag){
            // returns filtered value
            r += signal.readValue(1)/SAMPLES_PER_AVERAGE;
            l += signal.readValue(2)/SAMPLES_PER_AVERAGE;
            u += signal.readValue(3)/SAMPLES_PER_AVERAGE;
            d += signal.readValue(4)/SAMPLES_PER_AVERAGE;
            }
            if(i=10){ // the filtering works on a frequency which is 10 times higher
                
            // 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;
            
            double minXThreshold;
            double minYThreshold;
            
            r=0; l=0; u=0; d=0;
                // 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
                motorValueX = percentageR;
                minXThreshold = 0.2f*rMax; // set the threshold
            }   else {motorValueX = -percentageL;
                    minXThreshold = 0.2f*lMax;
                }
            if (percentageU > percentageD) { // vertical speed request
                motorValueY = percentageU;
                minYThreshold = 0.2f*uMax;
            }   else {motorValueY = -percentageD;
                    minYThreshold = 0.2f*dMax;
                }
            // current pulses
            xPulses = pulses.getDistanceX(); // in pulses
            yPulses = pulses.getDistanceY();
            
            //edge detection
            if (xPulses > (x_max-margin) && motorValueX > 0) { // right should correspond to positive motorvalue
                motorValueX = 0;
            }
            if (xPulses < margin && motorValueX < 0) { 
                motorValueX = 0;
            }
            if (yPulses > (y_max-margin) && motorValueY > 0) { // up should correspond to positive motorvalue
                motorValueY = 0;
            }
            if (yPulses < margin && motorValueY < 0) {
                motorValueY =0;
            } 
            
            // Quantize the linear speed request
            // motorValue is a value [-1,1], currently only 0 or 1/-1
            if (motorValueX > maxXValue){motorValueX = maxXValue;} // safety
            if (motorValueX < -maxXValue){motorValueX = -maxXValue;}
            if (motorValueX > minXThreshold && motorValueX < maxXValue){motorValueX = maxXValue;} // if threshold is met pwm is 100%, maybe quantize here
            if (motorValueX < -minXThreshold && motorValueX > -maxXValue){motorValueX = -maxXValue;}
                
            if (motorValueY > maxYValue){motorValueY = maxYValue;}
            if (motorValueY < -maxYValue){motorValueY = -maxYValue;}
            if (motorValueY > minYThreshold && motorValueY < maxYValue){motorValueY = maxYValue;}
            if (motorValueY < -minYThreshold && motorValueY > -maxYValue){motorValueY = -maxYValue;}
            

            // current position of pen in Meters
            float currentX = (pulses.getDistanceX()/(8400.0f))*(2*PI)*RmX;
            float currentY = (pulses.getDistanceY()/(8400.0f))*(2*PI)*Rpulley; 
            
            // upcoming position of pen in Meters
            //toX = currentX + motorValueX*maxSpeed*RATE; // speed is in m/s rate is in s
            //toY = currentY + motorValueY*maxSpeed*RATE; 
            
            // distance it should travel for 1 interval in Meters 
            float deltaX = motorValueX*maxSpeed*RATE;
            float deltaY = motorValueY*maxSpeed*RATE;
            
            // desired velocity in m/s for this interval
            float toVelX = deltaX/RATE; 
            float toVelY = deltaY/RATE;
            
            // set setpoint of PID controller, this is the thing you want
            X_Controller.setSetPoint(toVelX);
            Y_Controller.setSetPoint(toVelY);
            
            // current velocity in M/s, the thing you measure
            // horizontal motor
            float currVelX = (currentX - prevX)/RATE;
            prevX = currentX;
            X_Controller.setProcessValue(currVelX);
            
            //vertical motor
            float currVelY = (currentY - prevY)/RATE;
            prevY = currentY;
            Y_Controller.setProcessValue(currVelY);
            
                // compute the output FIXME[calibratebutton might break the program]
                float X_MotorOutput = X_Controller.compute();
                if (X_MotorOutput > 0){ // right request
                    X_Direction.write(CW);
                    X_Magnitude.write(X_MotorOutput); // must be value between 0 and 1
                }   else {
                        X_Direction.write(!CW); // left request
                        X_Magnitude.write(-X_MotorOutput);
                    }
                        
                float Y_MotorOutput = Y_Controller.compute();
                if (Y_MotorOutput > 0){ // up request
                    Y_Direction.write(CW);
                    Y_Magnitude.write(Y_MotorOutput);
                }   else {
                        Y_Direction.write(!CW); // down request
                        Y_Magnitude.write(-Y_MotorOutput);
                    }
            }
           
           // output to hidscope
           
            flag = false; // only output to motor with RATE
            i = 1;
        }
        
    }
}

void setFlag() {
    flag = true;
    i++;
}

void calibrate() {
    motorControl.detach();
    calibrateFlag = false;
    // GO-TO pos (0,0)
    while ((pulses.getDistanceX() > margin) && (pulses.getDistanceY() > margin)){
        if (pulses.getDistanceX() > margin){
        // go direction LEFT
        X_Direction.write(!CW);
        X_Magnitude.write(1.0);
        }   else { //motormagX 0 
            X_Direction.write(CW);
            X_Magnitude.write(0.0);   
            }
        if (pulses.getDistanceY() > margin){
        // go direction DOWN
        Y_Direction.write(!CW);
        Y_Magnitude.write(1.0);
        }   else { //motormagY 0  
            Y_Direction.write(CW);
            Y_Magnitude.write(0.0);   
            }
        // 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?

    }
    prevX = getDistanceX();
    prevY = getDistanceY();
    r=0; l=0; u=0; d=0;
    i=0;
    motorControl.attach(&setFlag, RATE); 
}   
 
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;
}