Diff: Controller.cpp

Revision:

4:bfdcf3da7cff

Parent:

2:6b913f93bc0b

Child:

6:a4440eec3652

--- a/Controller.cpp	Wed Oct 26 12:40:26 2016 +0000
+++ b/Controller.cpp	Fri Oct 28 12:49:02 2016 +0000
@@ -1,27 +1,51 @@
-#include "initialize.h"
 #include "Controller.h"
 
 int main() {
-    calibrateButton.rise(&calibrate);
-    motorControl.attach(&setFlag, RATE); // rate =0.001
+    //pc.baud(115200);
+    //pc.printf("startup: \n\r");
+    //calibrateButton.rise(&calibrate);
+    
     initMotors();
     initControllers();
-    calibrate(); // calibrate position
-    calibratePower(); // store max amplitude each emg signal
-    run();
+    //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() {
-    while(true){
-        if(flag){
+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;
-            }
-            if(i=10){ // the filtering works on a frequency which is 10 times higher
-                
+            
+            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;
@@ -32,37 +56,49 @@
             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
-        } 
+           
+            // ignore the weaker signal,motorValue calculation, threshold set
+         
         
-        if ( percentageR > percentageL){ // horizontal speed request
+            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
+                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;
-                }
+            }   
+            else 
+            {
+                motorValueY = -percentageD;
+                minYThreshold = 0.2f*dMax;
+            }
             // current pulses
-            xPulses = pulses.getDistanceX(); // in pulses
-            yPulses = pulses.getDistanceY();
+            xPulses = X_Motor.getPulses();
+            yPulses = Y_Motor.getPulses();
             
             //edge detection
-            if (xPulses > (x_max-margin) && motorValueX > 0) { // right should correspond to positive motorvalue
+            if (xPulses > (x_max-margin) && motorValueX > 0) 
+            { // right should correspond to positive motorvalue
                 motorValueX = 0;
             }
-            if (xPulses < margin && motorValueX < 0) { 
+            if (xPulses < margin && motorValueX < 0) 
+            { 
                 motorValueX = 0;
             }
-            if (yPulses > (y_max-margin) && motorValueY > 0) { // up should correspond to positive motorvalue
+            if (yPulses > (y_max-margin) && motorValueY > 0) 
+            { // up should correspond to positive motorvalue
                 motorValueY = 0;
             }
-            if (yPulses < margin && motorValueY < 0) {
+            if (yPulses < margin && motorValueY < 0) 
+            {
                 motorValueY =0;
             } 
             
@@ -78,10 +114,9 @@
             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; 
+            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
@@ -110,54 +145,52 @@
             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);
-                    }
+            // 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
             }
-           
-           // output to hidscope
-           
-            flag = false; // only output to motor with RATE
-            i = 1;
+                    
+            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 setFlag() {
-    flag = true;
-    i++;
+void setUpdate_flag() {
+    update_flag = true;
 }
 
 void calibrate() {
     motorControl.detach();
-    calibrateFlag = false;
     // GO-TO pos (0,0)
-    while ((pulses.getDistanceX() > margin) && (pulses.getDistanceY() > margin)){
-        if (pulses.getDistanceX() > margin){
+    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);
+        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 (pulses.getDistanceY() > margin){
+        if (Y_Motor.getPulses() > margin){
         // go direction DOWN
         Y_Direction.write(!CW);
         Y_Magnitude.write(1.0);
@@ -165,34 +198,35 @@
             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();
+    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;
-    motorControl.attach(&setFlag, RATE); 
+    i=0; 
 }   
  
 void calibratePower(){
-    while((clock()/CLOCKS_PER_SEC) < 5){ // FIXME
+    while((clock()/CLOCKS_PER_SEC) < 5)
+    { // FIXME
         // do this for 5 sec
-        double rCal = signal.readValue(1); 
-        if ( rCal > rMax){
+        float rCal = signal.readValue(1); 
+        if ( rCal > rMax)
+        {
             rMax = rCal;
         }
-        double lCal = signal.readValue(2);
-        if ( lCal > lMax){
+        float lCal = signal.readValue(2);
+        if ( lCal > lMax)
+        {
             lMax = lCal;
         }
-        double uCal = signal.readValue(3);
-        if ( uCal > uMax){
+        float uCal = signal.readValue(3);
+        if ( uCal > uMax)
+        {
             uMax = uCal;
         }
-        double dCal = signal.readValue(4);
-        if ( dCal > dMax){
+        float dCal = signal.readValue(4);
+        if ( dCal > dMax)
+        {
             dMax = dCal;
         }
     }