Diff: Controller.cpp

Revision:

2:6b913f93bc0b

Parent:

1:b9005f2aabf5

Child:

4:bfdcf3da7cff

--- a/Controller.cpp	Fri Oct 21 21:32:20 2016 +0000
+++ b/Controller.cpp	Wed Oct 26 12:37:37 2016 +0000
@@ -3,105 +3,137 @@
 
 int main() {
     calibrateButton.rise(&calibrate);
-    motorControl.attach(&setFlag, RATE);
+    motorControl.attach(&setFlag, RATE); // rate =0.001
     initMotors();
     initControllers();
+    calibrate(); // calibrate position
     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){
+            // 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 (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 ( 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
-                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;
+                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 (x > x_max && motorValueHor > 0) { // right should correspond to positive motorvalue
-                motorValueHor = 0;
+            if (xPulses > (x_max-margin) && motorValueX > 0) { // right should correspond to positive motorvalue
+                motorValueX = 0;
             }
-            if (x < 0 && motorValueHor < 0) { // carefulwith 0 pulses, maybe implement margin
-                motorValueHor = 0;
+            if (xPulses < margin && motorValueX < 0) { 
+                motorValueX = 0;
             }
-            if (y > y_max && motorValueVer > 0) { // up should correspond to positive motorvalue
-                motorValueVer = 0;
+            if (yPulses > (y_max-margin) && motorValueY > 0) { // up should correspond to positive motorvalue
+                motorValueY = 0;
             }
-            if (y < 0 && motorValueVer < 0) {
-                motorValueVer =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 (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 (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 (motorValueVer > maxVerValue){motorValueVer = maxVerValue;}
-            if (motorValueVer < -maxVerValue){motorValueVer = -maxVerValue;}
-            if (motorValueVer > minVerThreshold && motorValueVer < maxVerValue){motorValueVer = maxVerValue;}
-            if (motorValueVer < -minVerThreshold && motorValueVer > -maxVerValue){motorValueVer = -maxVerValue;}
+            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; 
             
-            //current position of pen in degrees
-            currentXangle = 360.0f*(pulses.getDistanceHor()/8400);
-            currentYangle = 360.0f*(pulses.getDistanceVer()/8400); 
+            // 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;
             
-            // 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 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);
             
-            // desired position of pen in cm
-            toX = currentX + motorValueHor*maxSpeed*RATE;
-            toY = currentY + motorValueVer*maxSpeed*RATE; 
+            // current velocity in M/s, the thing you measure
+            // horizontal motor
+            float currVelX = (currentX - prevX)/RATE;
+            prevX = currentX;
+            X_Controller.setProcessValue(currVelX);
             
-            // desired angle of pen in degrees
-            toangleX = ((toX-currentX)*(R1/RmHor))*(360.0f/(2*PI)); 
-            toangleY = (toX-currentX)*(360.0f/(2*PI)); 
+            //vertical motor
+            float currVelY = (currentY - prevY)/RATE;
+            prevY = currentY;
+            Y_Controller.setProcessValue(currVelY);
             
-            //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
+                // 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;
         }
         
     }
@@ -109,30 +141,39 @@
 
 void setFlag() {
     flag = true;
-}
-
-void emergencyExit() {
-    calibrate();
-    run();
+    i++;
 }
 
 void calibrate() {
-    while ((pulses.getDistanceHor() > margin) && (pulses.getDistanceVer() > margin)){
-        if (pulses.getDistanceHor() > margin){
+    motorControl.detach();
+    calibrateFlag = false;
+    // GO-TO pos (0,0)
+    while ((pulses.getDistanceX() > margin) && (pulses.getDistanceY() > margin)){
+        if (pulses.getDistanceX() > margin){
         // go direction LEFT
-        }
-        else { //motormaghor 0    
-        }
-        if (pulses.getDistanceVer() > margin){
+        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
-        }
-        else { //motormagver 0}     
-        }
-        // GO-TO pos (0,0)
-        // pulses can only be 0 or positive so reverse the direction until 0 pulses is reached
+        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(){