Diff: main.cpp

Revision:

6:3af4a77c9935

Parent:

5:455773cf460b

--- a/main.cpp	Tue Sep 29 11:54:47 2015 +0000
+++ b/main.cpp	Tue Sep 29 12:04:56 2015 +0000
@@ -13,11 +13,10 @@
 QEI Encoder(D3, D2, NC, 128);
 HIDScope scope(3);
 Ticker ScopeTime;
-float Aantal_Degs;
-float Aantal_pulses;
-float Error;
-float refference;
-const float Kp = 0.005;
+double Aantal_Degs;
+double Aantal_pulses;
+double Error;
+double refference;
 
 
 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
@@ -30,6 +29,10 @@
     scope.send();
     
 }
+double p_control(double kp, double Error)
+{
+ return Error*kp;
+}
 
 
 int main()
@@ -38,11 +41,10 @@
     motor2speed = 0;
     led = 1;
     pc.baud(115200);
-    float refference = 0;
+    double refference = 0;
     pc.printf("Tot aan loop werkt\n");
     ScopeTime.attach_us(&ScopeSend, 10e4);
     
-    
     while (true) 
     {
         
@@ -52,11 +54,13 @@
             refference = refference + 10;
             pc.printf("rx \n");
             Error = refference - Aantal_Degs;
+            double Output = p_control(0.05,Error);
+            
             while(abs(Error) > 2)
                 {
                     Error = refference - Aantal_Degs;
-                    motor2speed = Kp*abs(Error);
-                    pc.printf("reffence = %f,error = %f \n",refference,Error);
+                    motor2speed = abs(Output);
+                    pc.printf("reffence = %d,error = %d \n",refference,Error);
                     if(Error > 0)
                     {
                         motor2direction = 0;