Revision 1:19109095f9d5, committed 2016-10-14

Comitter:

s1588141

Date:

Fri Oct 14 14:38:55 2016 +0000

Parent:

0:272f7a0c3493

Commit message:

Onstabiele regelaar met P actie;

Changed in this revision

diff -r 272f7a0c3493 -r 19109095f9d5 main.cpp
--- a/main.cpp	Fri Oct 14 08:41:26 2016 +0000
+++ b/main.cpp	Fri Oct 14 14:38:55 2016 +0000
@@ -20,11 +20,12 @@
 const int SampleFrequency = 100;
 const float pi = 3.141;
 float Speed_L = 0.0;
-const float AnglePerPulse = (2.0f*pi)/8400;
+const float AnglePerPulse = (2*pi)/4200;
 double Position_L = 0.0;
 double Position_R = 0.0;
 double Previous_Position_L = 0.0;
 double Previous_Position_R = 0.0;
+float Signal_L = 2*pi;
 //--------------------Analog---------------------------------
 //-------------------Hidscope----------------------------------
 HIDScope scope(2); // Sending two sets of data
@@ -48,8 +49,11 @@
 //-----------------------Functions------------------------------
 //--------------------------------------------------------------
 bool Go_Flag = true;
+bool Cont_Flag = true;
+float Error = 0;
 void SetFlag(){
     Go_Flag = true;
+    Cont_Flag = true;
     }
 //-----------------------Interrupts-----------------------------
 void Boot(){ 
@@ -69,9 +73,6 @@
     //---------------------Sending data---------------------------------------------
         
         Speed_L = (Position_L-Previous_Position_L)*SampleFrequency;
-        if (Position_L >= pi){
-            M_L_S = 0;
-            }
 
         Encoder_L.reset();
         Encoder_R.reset();
@@ -81,12 +82,21 @@
         Go_Flag = false;
         }
     }
+void Controller(){//onstabiele gain regelaar
+    if (Cont_Flag == true ){
+        Error = Signal_L-Position_L ;
+        if (Error >=0){
+            M_L_D = 1;
+            M_L_S = fabs(Error);
+            }  else {
+            M_L_D = 0;
+            M_L_S = fabs(Error);
+            }
+        Cont_Flag = false;
+        }
+    }
 //-----------------------Encoders-----------------------------------------------
-int Get_Phase_L(){
-    int Phase = Encoder_L.getCurrentState();
-  
-    return Phase;
-    }
+
 int main()
 {
 //---------------------Setting constants and system booting--------------------
@@ -94,10 +104,11 @@
     pc.printf("\r\n**BOARD RESET**\r\n");
     Boot();
     Measure.attach(SetFlag, 1.0/SampleFrequency);     
-    M_L_S = 0.1;
+    M_L_D = 1;
 //------------------------Main While Loop--------------------------------------
     while (true) {  
     Information();
+    Controller();
        // control= pc.getc();     
     }
 }
\ No newline at end of file