Diff: Controller.cpp

Revision:

0:d935ea6f5c25

Child:

1:b9005f2aabf5

diff -r 000000000000 -r d935ea6f5c25 Controller.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Fri Oct 21 09:13:53 2016 +0000
@@ -0,0 +1,108 @@
+#include "initialize.h"
+#include "Controller.h"
+int main() {
+    calibrateButton.rise(&calibrate);
+    motorControl.attach(&setFlag, RATE);
+    //[FIXME]calibratePowerrrr(); 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) {
+            // ignore the weaker signal and motorValue is calculate here
+            if ( percentageR > percentageL){ // horizontal speed
+                motorValueHor = percentageR;
+            } else {motorValueHor = -percentageL;}
+            if (percentageU > percentageD) { // vertical speed 
+                motorValueVer = percentageU;
+            } else {motorValueVer = -percentageD;}
+            
+            // 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();
+            }
+            if (y > y_max || y < 0) { //
+                emergencyExit(); 
+            }
+            
+            // 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;}
+                
+            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;}
+            
+            // Calculate desired rotational velocity from linear velocity
+            
+            // Calculate current rotational velocity
+            
+            // give the PID controller the setpoint and 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
+        }
+        
+    }
+}
+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;
+}
+
+void emergencyExit() {
+    calibrate();
+    run();
+ }   
\ No newline at end of file