Diff: Controller.cpp

Revision:

1:b9005f2aabf5

Parent:

0:d935ea6f5c25

Child:

2:6b913f93bc0b

--- a/Controller.cpp	Fri Oct 21 09:13:53 2016 +0000
+++ b/Controller.cpp	Fri Oct 21 21:32:20 2016 +0000
@@ -1,9 +1,12 @@
 #include "initialize.h"
 #include "Controller.h"
+
 int main() {
     calibrateButton.rise(&calibrate);
     motorControl.attach(&setFlag, RATE);
-    //[FIXME]calibratePowerrrr(); store max amplitude each emg signal
+    initMotors();
+    initControllers();
+    calibratePower(); // store max amplitude each emg signal
     run();
 }
 
@@ -21,51 +24,81 @@
         double percentageD = d/dMax;
         
         
-        if (flag) {
-            // ignore the weaker signal and motorValue is calculate here
-            if ( percentageR > percentageL){ // horizontal speed
+        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;
-            } else {motorValueHor = -percentageL;}
-            if (percentageU > percentageD) { // vertical speed 
+                minHorThreshold = 0.2*rMax; // set the threshold
+            } else {motorValueHor = -percentageL;
+                  minHorThreshold = 0.2*lMax;
+              }
+            if (percentageU > percentageD) { // vertical speed request
                 motorValueVer = percentageU;
-            } else {motorValueVer = -percentageD;}
-            
+                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;
             
-            //edge detection: for now it will calibrate; implement function that allows movement for the other directions 
-            if (x > x_max || x < 0) { 
-                emergencyExit();
+            //edge detection
+            if (x > x_max && motorValueHor > 0) { // right should correspond to positive motorvalue
+                motorValueHor = 0;
+            }
+            if (x < 0 && motorValueHor < 0) { // carefulwith 0 pulses, maybe implement margin
+                motorValueHor = 0;
             }
-            if (y > y_max || y < 0) { //
-                emergencyExit(); 
+            if (y > y_max && motorValueVer > 0) { // up should correspond to positive motorvalue
+                motorValueVer = 0;
             }
+            if (y < 0 && motorValueVer < 0) {
+                motorValueVer =0;
+            } 
             
-            // Quantize
-            if (motorValueHor > maxHorVel){motorValueHor = maxHorVel;}
-            if (motorValueHor < -maxHorVel){motorValueHor = -maxHorVel;}
-            if (motorValueHor > 0 && motorValueHor < maxHorVel){motorValueHor = 0.5*maxHorVel;}
-            if (motorValueHor < 0 && motorValueHor > -maxHorVel){motorValueHor = -0.5*maxHorVel;}
+            // 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 ( motorValueVer > maxVerVel){motorValueVer = 1;}
-            if (motorValueVer < -1){motorValueVer = -1;}
-            if (motorValueVer > 0 && motorValueVer < 1){motorValueVer = 0.5;}
-            if (motorValueVer < 0 && motorValueVer > -1){motorValueVer = -0.5;}
+            if (motorValueVer > maxVerValue){motorValueVer = maxVerValue;}
+            if (motorValueVer < -maxVerValue){motorValueVer = -maxVerValue;}
+            if (motorValueVer > minVerThreshold && motorValueVer < maxVerValue){motorValueVer = maxVerValue;}
+            if (motorValueVer < -minVerThreshold && motorValueVer > -maxVerValue){motorValueVer = -maxVerValue;}
+            
+            //current position of pen in degrees
+            currentXangle = 360.0f*(pulses.getDistanceHor()/8400);
+            currentYangle = 360.0f*(pulses.getDistanceVer()/8400); 
+            
+            // 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??
             
-            // Calculate desired rotational velocity from linear velocity
+            // desired position of pen in cm
+            toX = currentX + motorValueHor*maxSpeed*RATE;
+            toY = currentY + motorValueVer*maxSpeed*RATE; 
+            
+            // desired angle of pen in degrees
+            toangleX = ((toX-currentX)*(R1/RmHor))*(360.0f/(2*PI)); 
+            toangleY = (toX-currentX)*(360.0f/(2*PI)); 
+            
+            //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 setpoint and processvariable
+            // give the PID controller the processvariable
             
             // compute PID
             
             // output to motor
-           // motorOutput(toMotorValue(percentageR, percentageL), true); // [FIXME]- safety edges (if statement)
-           // motorOutput(toMotorValue(percentageU, percentageD), false); // replace this with all the calculations and PID class stuff
            
            // output to hidscope
             flag = false; // only output to motor with RATE
@@ -73,30 +106,6 @@
         
     }
 }
-void calibrate() {
-    // GO-TO pos (0,0)
-    // pulses can only be 0 or positive so reverse the direction until 0 pulses is reached
-    // 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?
-    return;
-}
-
-void motorOutput(double motorValue, bool horizontal) { // IMPLEMENT DEADZONE?
-    if (motorValue > 1) { // safety
-        motorValue = 1;
-    }
-    if (motorValue < -1) {
-        motorValue = -1;
-    }
-    
-    // deadzone here
-    
-    if (horizontal) {
-        motor.outputHor(fabs(motorValue), (motorValue>0) );
-    } else {
-        motor.outputVer(fabs(motorValue), (motorValue>0));
-    }
-}
 
 void setFlag() {
     flag = true;
@@ -105,4 +114,46 @@
 void emergencyExit() {
     calibrate();
     run();
- }   
\ No newline at end of file
+}
+
+void calibrate() {
+    while ((pulses.getDistanceHor() > margin) && (pulses.getDistanceVer() > margin)){
+        if (pulses.getDistanceHor() > margin){
+        // go direction LEFT
+        }
+        else { //motormaghor 0    
+        }
+        if (pulses.getDistanceVer() > 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
+        // 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?
+    }
+}   
+ 
+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;
+}