File content as of revision 6:a4440eec3652:

#include "Controller.h"

int main() {
    pc.baud(115200);
    initMotors();
    initControllers();
    calibratePower(); // store max amplitude each emg signal

    while(true)
    {
        calibrate(); // calibrate position
        while(!button){} // button not pressed is true, this is here to make sure that the it will only go run if you have let go of the button and calibrated
        run();
    }
}

void run() 
{
    //pc.printf("ik ga nu runnen G \n\r");
    motorControl.attach(&setUpdate_flag, RATE); // rate =0.001
    while(button) // button not presseed
    {
        if(update_flag) //
        { 
            // returns filtered value
            r = signal.readValue(1); 
            l = signal.readValue(2);
            //pc.printf("rechts is : %f\n\r", r);
            //pc.printf("links is  : %f\n\r", l);
            
            //u = signal.readValue(3);
           // d = signal.readValue(4);
            // calculate r,l,u,d relative to the max [0,1]
            percentageR = r/rMax;
            percentageL = l/lMax;
            percentageU = u/uMax;
            percentageD = d/dMax;
            //scope.set(0,percentageR);
            //scope.set(1,percentageL);
            //scope.send();
            //pc.printf("wat is percentageL : %f \n\r", percentageL);
            // ignore the weaker signal,motorValue calculation, threshold set
            if ( percentageR > percentageL)
            { // horizontal speed request
                motorValueX = percentageR;
                //pc.printf("rechts > links : %f\n\r", motorValueX);
            }   
            else 
            {
                motorValueX = -percentageL;
                //pc.printf("rechts < links : %f\n\r", motorValueX);
            }
            if (percentageU > percentageD) // vertical speed request
            { 
                motorValueY = percentageU;
            }   
            else 
            {
                motorValueY = -percentageD;
            }
            
            //pc.printf("motorValueX : %f\n\r", motorValueX);
            // current pulses
            xPulses = X_Motor.getPulses();
           // pc.printf("curentpulses : %f \n\r", xPulses);
            yPulses = Y_Motor.getPulses();
            
            //edge detection
            if ((xPulses > (x_max-margin)) && (motorValueX > sensitivityFactor)) // (x_max-margin
            { // right should correspond to positive motorvalue
                motorValueX = 0;
            }
            if ((xPulses < margin) && (motorValueX < (-1 * sensitivityFactor))) 
            { 
                motorValueX = 0;
            }
            if ((yPulses > (y_max-margin)) && (motorValueY > sensitivityFactor)) 
            { // up should correspond to positive motorvalue
                motorValueY = 0;
            }
            if ((yPulses < margin) && (motorValueY < -sensitivityFactor)) 
            {
                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
                else if (motorValueX <= -maxXValue){motorValueX = -maxXValue;}
                    else if ((motorValueX >= sensitivityFactor) && (motorValueX < maxXValue)){motorValueX = maxXValue;} // if threshold is met pwm is 100%, maybe quantize here
                        else if ((motorValueX <= -sensitivityFactor) && (motorValueX > -maxXValue)){motorValueX = -maxXValue;}
                            else {motorValueX = 0;}
            if (motorValueY >= maxYValue){motorValueY = maxYValue;}
                else if (motorValueY <= -maxYValue){motorValueY = -maxYValue;}
                    else if ((motorValueY >= sensitivityFactor) && (motorValueY <= maxYValue)){motorValueY = maxYValue;}
                        else if ((motorValueY <= -sensitivityFactor) && (motorValueY >= -maxYValue)){motorValueY = -maxYValue;}
                            else {motorValueX = 0;}
            //pc.printf("dit moet nu value hebben als je aanspant : %f \n\r", motorValueX);        
            // current position of pen in Meters
            float currentX = (xPulses/(8400.0f))*(2*PI)*RmX;
            float currentY = (yPulses/(8400.0f))*(2*PI)*Rpulley; 
            //pc.printf("currentX : %f \n\r", currentX);
            
            // 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;
            //pc.printf("deltaX : %f \n\r", deltaX);
            // desired velocity in m/s for this interval
            float toVelX = deltaX/RATE; 
            float toVelY = deltaY/RATE;
            //pc.printf("toVelX : %f \n\r", toVelX);
            // 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;
            //pc.printf("currVe;X : %f \n\r", currVelX);
            X_Controller.setProcessValue(currVelX);
            
            //vertical motor
            float currVelY = (currentY - prevY)/RATE;
            prevY = currentY;
            Y_Controller.setProcessValue(currVelY);
            
            // compute the output
            float X_MotorOutput = 10*(X_Controller.compute());
            pc.printf("X_MotorOutput : %f \n\r", X_MotorOutput);
            
            if (X_MotorOutput >= X_frictionTrheshold) // hardcoded threshold internal friction
            { // right request
                X_Direction.write(CW);
                X_Magnitude.write(X_MotorOutput); // must be value between 0 and 1
            }   
            else if (X_MotorOutput <= -X_frictionTrheshold)
            {
                X_Direction.write(!CW); // left request
                X_Magnitude.write(fabs(X_MotorOutput)); //motor output will be negative and you need to write a pos value
            }
            else // else the internal friction is not compensated
            {
                X_Magnitude.write(0);
                X_Direction.write(CW);
            }
            
            float Y_MotorOutput = Y_Controller.compute();
            if (Y_MotorOutput >= Y_frictionTrheshold)
            { // up request
                Y_Direction.write(CW);
                Y_Magnitude.write(Y_MotorOutput);
            }   
            else if (Y_MotorOutput <= -Y_frictionTrheshold) 
            {
                Y_Direction.write(!CW); // down request
                Y_Magnitude.write(fabs(Y_MotorOutput));
            }
            else
            {
                Y_Magnitude.write(0);
                Y_Direction.write(CW);
            }
            update_flag = false;
        }
    }
    motorControl.detach();
}

void setUpdate_flag() {
    update_flag = true;
}

void calibrate() {
    
    // GO-TO pos (0,0)
    while ((X_Motor.getPulses() > margin) || (Y_Motor.getPulses() > margin)){
        float pulses = X_Motor.getPulses();
        printf("pulsesX : %f\n\r", pulses);
        if (X_Motor.getPulses() > margin)
        {
        // go direction LEFT
            X_Direction.write(!CW);
            X_Magnitude.write(0.4); // 8 RPM with no load, maybe compensate for internal friction
        }   
        else 
        { //motormagX 0 
            X_Direction.write(CW);
            X_Magnitude.write(0.0);   
        }
        if (Y_Motor.getPulses() > margin)
        {
        // go direction DOWN
            Y_Direction.write(!CW);
            Y_Magnitude.write(0.4);
        }   
        else 
        { //motormagY 0  
            Y_Direction.write(CW);
            Y_Magnitude.write(0.0);   
        }
    }
    prevX = (X_Motor.getPulses()/(8400.0f))*(2*PI)*RmX;
    prevY = (X_Motor.getPulses()/(8400.0f))*(2*PI)*Rpulley; 
}   
 
void calibratePower(){ 
    motorControl.attach(&setUpdate_flag, RATE);
    led = false; // contract now, false = on
    while (counter < 3000) // get 3000 samples for the max
    {
        if(update_flag)
        {
            float rCal = signal.readValue(1); 
            if ( rCal > rMax)
            {
                rMax = rCal;
            }
            
            float lCal = signal.readValue(2);
            if ( lCal > lMax)
            {
                lMax = lCal;
            }
            /*
            float uCal = signal.readValue(3);
            if ( uCal > uMax)
            {
                uMax = uCal;
            }   
            float dCal = signal.readValue(4);
            if ( dCal > dMax)
            {
                dMax = dCal;
            }
            */
            counter++;
            //pc.printf("rCal: %f \n\r", rCal);
            
            update_flag = false;
        }  
    }
    counter = 0;
    pc.printf("rMax : %f\n\r", rMax);
    pc.printf("lMax : %f\n\r", lMax);
    motorControl.detach();
    led = true;
    //pc.printf("ik moet hier led weer uitzetten \n\r");

}