File content as of revision 5:623bab832e6a:

#include "Controller.h"

int main() {
    //pc.baud(115200);
    //pc.printf("startup: \n\r");
    //calibrateButton.rise(&calibrate);
    
    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() 
{
    
    motorControl.attach(&setUpdate_flag, RATE); // rate =0.001
    while(button) // button not presseed
    {
       //pc.printf("run: %d\n\r", counter);
       
        if(update_flag)
        {
            led = !led;
            //counter++;
            // returns filtered value
            r += signal.readValue(1)/SAMPLES_PER_AVERAGE;
            float r1= signal.readValue(1);
            l += signal.readValue(2)/SAMPLES_PER_AVERAGE;
            u += signal.readValue(3)/SAMPLES_PER_AVERAGE;
            d += signal.readValue(4)/SAMPLES_PER_AVERAGE;
            
            scope.set(0,r);
            scope.set(1,r1);
            scope.send();
            i++;
            update_flag = false;
        }
        if(i==SAMPLES_PER_AVERAGE) // the filtering works on a frequency which is 10 times higher, 
        { 
        //counter_motor++;
        //scope.set(0,counter_motor); // emg1 

            // 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;
           
            // ignore the weaker signal,motorValue calculation, threshold set
         
        
            if ( percentageR > percentageL)
            { // horizontal speed request
                motorValueX = percentageR;
                minXThreshold = 0.2f*rMax; // set the threshold for quantizer
            }   
            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 = X_Motor.getPulses();
            yPulses = Y_Motor.getPulses();
            
            //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 = (X_Motor.getPulses()/(8400.0f))*(2*PI)*RmX;
            float currentY = (X_Motor.getPulses()/(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
            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); //motor output will be negative and you need to write a pos value
            }
                    
            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);
            }
            i = 0;
        }
    }
}

void setUpdate_flag() {
    update_flag = true;
}

void calibrate() {
    motorControl.detach();
    // GO-TO pos (0,0)
    while ((X_Motor.getPulses() > margin) && (Y_Motor.getPulses() > margin)){
        if (X_Motor.getPulses() > margin){
        // go direction LEFT
        X_Direction.write(!CW);
        X_Magnitude.write(1.0); //  WRONG, reqVel must be 1cm/s here
        }   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(1.0);
        }   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; 
    r=0; l=0; u=0; d=0;
    i=0; 
}   
 
void calibratePower(){
    while((clock()/CLOCKS_PER_SEC) < 5)
    { // FIXME
        // do this for 5 sec
        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;
        }
    }
    return;
}