Diff: Controller.h

Revision:

1:b9005f2aabf5

Parent:

0:d935ea6f5c25

Child:

2:6b913f93bc0b

--- a/Controller.h	Fri Oct 21 09:13:53 2016 +0000
+++ b/Controller.h	Fri Oct 21 21:32:20 2016 +0000
@@ -1,33 +1,101 @@
 InterruptIn calibrateButton(D2);
-int x = 0; // pulses 
-int x_max; // ???physically measure
+
+const double PI = 3.141592653589793;
+
+int x = 0; // pulsecount
+int x_max; // ??? physically measure
 int y = 0;
 int y_max; // ???
-double rMax; // calibrate maxpower..
+
+double rMax; // maxAmplitude emg signal
 double lMax;
 double uMax;
 double dMax;
-float RATE; // ???
-float motorValueHor = 0;
+
+float minHorThreshold; // threshold emgsignal
+float minVerThreshold;
+
+float motorValueHor = 0; // requested speed
 float motorValueVer = 0;
 
-float maxHorVel = 1.0f; // 1 cm/sec
-float maxVerVel = 1.0f; // 1cm/sec
+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 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;
 
-int prevX = 0;
-int prevY = 0;
+float currentXangle = 0; // degrees
+float currentYangle = 0;
+
+float toangleX = 0; // degrees
+float toangleY = 0; 
+
+float Xvelrequest = 0; // degrees/sec
+float Yvelrequest = 0;
+
+float prevX = 0; // pulses
+float prevY = 0;
+
+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();
 
 SigInterpreter signal = SigInterpreter();
-MyMotor motor = MyMotor();
 PulseChecker pulses =  PulseChecker();
 
+const float Kc = 0.0;
+const float Ti = 0.0;
+const float Td = 0.0;
+float RATE = 1/1000; // ???
+
+PwmOut horizontalMagnitude(D5); // bind to horizontal motor
+DigitalOut horizontalDirection(D4);
+PID horizontalController(Kc, Ti, Td, RATE);
+
+PwmOut verticalMagnitude(D6); // bind to vertical motor
+DigitalOut verticalDirection(D7);
+PID verticalController(Kc, Ti, Td, RATE);
+
 Ticker motorControl;
 void setFlag();
 bool flag = true;
+
+void initMotors(){
+    horizontalMagnitude.period_us(50);
+    horizontalMagnitude.write(0.0);
+    horizontalDirection.write(1);
+    
+    verticalMagnitude.period_us(50);
+    verticalMagnitude.write(0.0);
+    verticalDirection.write(1);
+}
+void initControllers(){
+    horizontalController.setInputLimits(-90.0,90.0); // fix me
+    horizontalController.setOutputLimits(-1.0,1.0);
+    horizontalController.setBias(0);
+    horizontalController.setMode(0);
+    
+    verticalController.setInputLimits(-90.0,90.0); // fix me
+    verticalController.setOutputLimits(-1.0,1.0);
+    verticalController.setBias(0);
+    verticalController.setMode(0);
+}