Revision 43:6d6c643d3e6d, committed 2016-10-28

Comitter:

RiP

Date:

Fri Oct 28 10:35:58 2016 +0000

Parent:

42:37cd882e7f2b

Commit message:

calibration toegevoegd

Changed in this revision

--- a/main.cpp	Thu Oct 27 14:26:50 2016 +0000
+++ b/main.cpp	Fri Oct 28 10:35:58 2016 +0000
@@ -26,6 +26,8 @@
 QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder
 QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder
 
+DigitalIn button(PTA4);
+
 //Define the Tickers
 Ticker      pos_timer;                      // the timer which is used to print the position every second
 Ticker      sample_timer;                   // the timer which is used to decide when a sample needs to be taken
@@ -34,6 +36,7 @@
 //Initialize all variables
 volatile bool sampletimer = false;          // a variable which is changed when a sample needs to be taken
 volatile bool controller_go=false;
+bool button_pressed=false;
 
 double threshold = 0.04;                    // the threshold which the emg signals need to surpass to do something
 double samplefreq=0.002;                    // every 0.002 sec a sample will be taken this is a frequency of 500 Hz
@@ -48,8 +51,9 @@
 double theta=0.0;                           // angle of the arm
 double radius=0.0;                          // radius of the arm
 
-const double minRadius=0.3;                 // minimum radius of arm
+double minRadius=0.3;                 // minimum radius of arm
 const double maxRadius=0.6;                 // maximum radius of arm
+const double min_Radius=0.3;
 const double minAngle=-1.25;                // minimum angle for limiting controller
 const double min_y=-0.28;                   // minimum height which the spatula can reach
 char key;                                   // variable to place the keyboard input
@@ -68,7 +72,7 @@
 const double pi=3.14159265359;
 const double res = 64/(1/131.25*2*pi);     // resolution on gearbox shaft per pulse
 const double V_max=9.0;                    // maximum voltage supplied by trafo
-const double pulleyDiameter=0.0398;        // pulley diameter
+const double pulleyRadius=0.0398/2;        // pulley diameter
 
 //Define the needed Biquad chains
 BiQuadChain bqc11;
@@ -172,8 +176,8 @@
     }
 
     // convert the x and y reference to the theta and radius reference
-    theta=atan(ref_y/(ref_x+minRadius));
-    radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
+    theta=atan(ref_y/(ref_x+min_Radius));
+    radius=sqrt(pow(ref_x+min_Radius,2)+pow(ref_y,2));
 
     //look if the new reference is outside the possible range and revert back to the old reference if it is outside  the range
     if (theta < minAngle) {
@@ -191,8 +195,8 @@
         ref_x=old_ref_x;
         ref_y=old_ref_y;
     }
-    theta=atan(ref_y/(ref_x+minRadius));
-    radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
+    theta=atan(ref_y/(ref_x+min_Radius));
+    radius=sqrt(pow(ref_x+min_Radius,2)+pow(ref_y,2));
 }
 
 double PID( double err, const double Kp, const double Ki, const double Kd,
@@ -216,7 +220,7 @@
 
     //converting radius and theta to gearbox angle
     double ref_angle1=16*theta;
-    double ref_angle2=(-radius+minRadius)/pi/pulleyDiameter;
+    double ref_angle2=(-radius+min_Radius)/pulleyRadius;
 
     double angle1 = Encoder1.getPulses()/res;   //get number of pulses (counterclockwise is positive)
     double angle2 = Encoder2.getPulses()/res;   //get number of pulses
@@ -285,10 +289,23 @@
 
     while(1) {
         //Only take a sample when the go flag is true.
+        if (button==0) {
+            button_pressed=true;
+            minRadius=-50.0;
+        } else {
+            if (button_pressed==true) {
+                ref_x=0.0;
+                ref_y=0.0;
+                button_pressed=false;
+            }
+            minRadius=0.3;
+        }
+
         if (sampletimer==true) {
             sample();
             sampletimer = false;            //change sampletimer to false if sample() is finished
         }
+
         if(controller_go) { // go flag
             controller();
             controller_go=false;