Diff: Controller.h

Revision:

6:a4440eec3652

Parent:

4:bfdcf3da7cff

Child:

8:82e38f4aa690

--- a/Controller.h	Fri Oct 28 12:50:44 2016 +0000
+++ b/Controller.h	Wed Nov 02 15:39:10 2016 +0000
@@ -2,27 +2,28 @@
 
 SigInterpreter signal = SigInterpreter();
 const double PI = 3.141592653589793;
-//MODSERIAL pc(USBTX, USBRX);
+
+MODSERIAL pc(USBTX, USBRX);
+//HIDScope scope(2);
 
 int xPulses = 0; // pulsecount
-const int x_max = 33068;
+const int x_max = 30068;// 30068;
 int yPulses = 0;
-const int y_max = 17640;
+const int y_max = 15003;
 int prevXpulses = 0;
 int prevYpulses = 0;
 
-double rMax = 0; // maxAmplitude emg signal
-double lMax = 0;
-double uMax = 0;
-double dMax = 0;
-
 float motorValueX = 0; // requested speed
 float motorValueY = 0;
 
-const float maxXValue = 1.0f; // v1 in drawing, max speed cm/s
-const float maxYValue = 1.0f; // 
+const float maxXValue = 1.0; // max motorvalue
+const float maxYValue = 1.0; 
 
-const float maxSpeed = 0.01f; // m/s
+const float maxSpeed = 1; // cm/s
+
+float X_frictionTrheshold = 0.04;
+float Y_frictionTrheshold = 0.04; 
+float sensitivityFactor = 0.2; // determines treshold value for contraction, value between 0 and 1
 
 const float R1 = 0.085; // radius of big driven gear in m
 const float RmX = 0.012; // radius of small drive gear in m   
@@ -31,51 +32,53 @@
 float prevX = 0;
 float prevY = 0;
 
-double r = 0;
-double l = 0;
-double u = 0;
-double d = 0;
+float r = 0;
+float l = 0;
+float u = 0;
+float d = 0;
+float rMax = 0; // maxAmplitude emg signal
+float lMax = 0;
+float uMax = 0;
+float dMax = 0;
+float percentageR = 0;
+float percentageL = 0;
+float percentageU = 0;
+float percentageD = 0;
+
 int i = 0;
 
-const int SAMPLES_PER_AVERAGE = 10;
 const int margin = 100; // margin pulses for calibration
 
 void calibrate();
 void calibratePower();
 void run();
-double toMotorValue(double,double);
-void emergencyExit();
-void motorOutput(double, bool);
 void initControllers();
 void initMotors();
-void initMotorvariables();
 
-HIDScope scope(2);
-
-const float Kc = 0.0;
+const float KcX = 0.0358; // THIS IS THE LIMIT FOR STABILITY
+const float KcY = 0.366;
 const float Ti = 0.0;
 const float Td = 0.0;
-const float RATE = 0.001; // rate = interval motors will be updateed
+const float RATE = 0.002; // rate = interval motors will be updateed
 
-int counter = 0;
-int counter_motor = 0;
+int counter = 1;
 
 PwmOut X_Magnitude(D5); // bind to horizontal motor
 DigitalOut X_Direction(D4);
-PID X_Controller(Kc, Ti, Td, RATE);
-QEI X_Motor(D12 ,D13 ,NC, 64 ,QEI::X4_ENCODING); 
+PID X_Controller(KcX, Ti, Td, RATE);
+QEI X_Motor(D13 ,D12 ,NC, 64 ,QEI::X4_ENCODING); 
 
-PwmOut Y_Magnitude(D6); // bind to Ytical motor
+PwmOut Y_Magnitude(D6); // bind to vertical motor
 DigitalOut Y_Direction(D7);
-PID Y_Controller(Kc, Ti, Td, RATE);
-QEI Y_Motor(D14 ,D15 ,NC, 64 ,QEI::X4_ENCODING); 
+PID Y_Controller(KcY, Ti, Td, RATE);
+QEI Y_Motor(D15 ,D14 ,NC, 64 ,QEI::X4_ENCODING); 
 
 //InterruptIn calibrateButton(D2);
 DigitalIn button(D2);
 Ticker motorControl;
 void setUpdate_flag();
 volatile bool update_flag = false;
-const int CW = true;
+const int CW = false;
 
 DigitalOut led(LED_BLUE);
 
@@ -87,15 +90,16 @@
     Y_Magnitude.period_ms(1);
     Y_Magnitude.write(0.0);
     Y_Direction.write(CW);
+    led = true;
 }
 void initControllers(){
-    X_Controller.setInputLimits(-1.0,1.0); // fiX_ me
+    X_Controller.setInputLimits(-maxSpeed,maxSpeed); // max output will be 10%
     X_Controller.setOutputLimits(-1.0,1.0);
     X_Controller.setBias(0);
-    X_Controller.setMode(0); // fix me
+    X_Controller.setMode(1); 
     
-    Y_Controller.setInputLimits(-1.0,1.0); // fix me
+    Y_Controller.setInputLimits(-maxSpeed,maxSpeed); // fix me
     Y_Controller.setOutputLimits(-1.0,1.0);
     Y_Controller.setBias(0);
-    Y_Controller.setMode(0);
+    Y_Controller.setMode(1);
 }