Diff: Controller.h

Revision:

2:6b913f93bc0b

Parent:

1:b9005f2aabf5

Child:

4:bfdcf3da7cff

--- a/Controller.h	Fri Oct 21 21:32:20 2016 +0000
+++ b/Controller.h	Wed Oct 26 12:37:37 2016 +0000
@@ -1,53 +1,39 @@
-InterruptIn calibrateButton(D2);
-
 const double PI = 3.141592653589793;
 
-int x = 0; // pulsecount
-int x_max; // ??? physically measure
-int y = 0;
-int y_max; // ???
+int xPulses = 0; // pulsecount
+int x_max = 33068;
+int yPulses = 0;
+int y_max = 17640;
+int prevXpulses = 0;
+int prevYpulses = 0;
 
 double rMax; // maxAmplitude emg signal
 double lMax;
 double uMax;
 double dMax;
 
-float minHorThreshold; // threshold emgsignal
-float minVerThreshold;
-
-float motorValueHor = 0; // requested speed
-float motorValueVer = 0;
+float motorValueX = 0; // requested speed
+float motorValueY = 0;
 
-const float maxHorValue = 1.0f; // v1 in drawing, max speed cm/s
-const float maxVerValue = 1.0f; // 
-
-const float maxSpeed = 1.0f; // cm/s
-
-const float R1 = 8.5; // radius of big driven gear in Cm
-const float RmHor = 0.5; // radius of small drive gear in Cm   
-const float RmVer = 1.5915; // radius of pulley gear in Cm
+const float maxXValue = 1.0f; // v1 in drawing, max speed cm/s
+const float maxYValue = 1.0f; // 
 
-const float maxRotSpeedHor = maxSpeed/RmHor; // w_m = v1/r2 rad/s 
-const float maxRotSpeedVer = maxSpeed/RmVer; // w_m = v1/r1 rad/s
-
-float currentX = 0; // CM
-float currentY = 0;
-
-float toX = 0; // CM
-float toY = 0;
+const float maxSpeed = 0.01f; // m/s
 
-float currentXangle = 0; // degrees
-float currentYangle = 0;
+const float R1 = 0.085; // radius of big driven gear in m
+const float RmX = 0.012; // radius of small drive gear in m   
+const float Rpulley = 0.015915; // radius of pulley gear in m
 
-float toangleX = 0; // degrees
-float toangleY = 0; 
-
-float Xvelrequest = 0; // degrees/sec
-float Yvelrequest = 0;
-
-float prevX = 0; // pulses
+float prevX = 0;
 float prevY = 0;
 
+double r = 0;
+double l = 0;
+double u = 0;
+double d = 0;
+int i = 1;
+
+int SAMPLES_PER_AVERAGE = 10;
 const int margin = 100; // margin pulses for calibration
 
 void calibrate();
@@ -58,6 +44,7 @@
 void motorOutput(double, bool);
 void initControllers();
 void initMotors();
+void initMotorvariables();
 
 SigInterpreter signal = SigInterpreter();
 PulseChecker pulses =  PulseChecker();
@@ -65,37 +52,41 @@
 const float Kc = 0.0;
 const float Ti = 0.0;
 const float Td = 0.0;
-float RATE = 1/1000; // ???
+float RATE = 0.01; // rate = interval motors will be updateed
+
+PwmOut X_Magnitude(D5); // bind to horizontal motor
+DigitalOut X_Direction(D4);
+PID X_Controller(Kc, Ti, Td, RATE);
 
-PwmOut horizontalMagnitude(D5); // bind to horizontal motor
-DigitalOut horizontalDirection(D4);
-PID horizontalController(Kc, Ti, Td, RATE);
+PwmOut Y_Magnitude(D6); // bind to Ytical motor
+DigitalOut Y_Direction(D7);
+PID Y_Controller(Kc, Ti, Td, RATE);
 
-PwmOut verticalMagnitude(D6); // bind to vertical motor
-DigitalOut verticalDirection(D7);
-PID verticalController(Kc, Ti, Td, RATE);
-
+InterruptIn calibrateButton(D2);
 Ticker motorControl;
 void setFlag();
 bool flag = true;
+bool calibrateFlag = true;
+
+int CW = true;
 
 void initMotors(){
-    horizontalMagnitude.period_us(50);
-    horizontalMagnitude.write(0.0);
-    horizontalDirection.write(1);
+    X_Magnitude.period_us(50);
+    X_Magnitude.write(0.0);
+    X_Direction.write(CW);
     
-    verticalMagnitude.period_us(50);
-    verticalMagnitude.write(0.0);
-    verticalDirection.write(1);
+    Y_Magnitude.period_us(50);
+    Y_Magnitude.write(0.0);
+    Y_Direction.write(CW);
 }
 void initControllers(){
-    horizontalController.setInputLimits(-90.0,90.0); // fix me
-    horizontalController.setOutputLimits(-1.0,1.0);
-    horizontalController.setBias(0);
-    horizontalController.setMode(0);
+    X_Controller.setInputLimits(-90.0,90.0); // fiX_ me
+    X_Controller.setOutputLimits(-1.0,1.0);
+    X_Controller.setBias(0);
+    X_Controller.setMode(0);
     
-    verticalController.setInputLimits(-90.0,90.0); // fix me
-    verticalController.setOutputLimits(-1.0,1.0);
-    verticalController.setBias(0);
-    verticalController.setMode(0);
+    Y_Controller.setInputLimits(-90.0,90.0); // fix me
+    Y_Controller.setOutputLimits(-1.0,1.0);
+    Y_Controller.setBias(0);
+    Y_Controller.setMode(0);
 }