Diff: main.cpp

Revision:

9:774fc3c6a39e

Parent:

8:69bde5e32dbf

Child:

10:80fe931a71e4

--- a/main.cpp	Tue Sep 29 12:45:26 2015 +0000
+++ b/main.cpp	Tue Sep 29 13:25:09 2015 +0000
@@ -4,22 +4,19 @@
 #include "QEI.h"
  
 Serial pc(USBTX, USBRX); // tx, rx
-DigitalOut led(LED_RED);
 DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
 PwmOut motor2speed(D5);
 DigitalIn button1(SW3);
-DigitalIn EncoderA(D3);
-DigitalIn EncoderB(D2);
+DigitalIn encoderA(D3);
+DigitalIn encoderB(D2);
+AnalogIn potmeter2(A5);
 QEI Encoder(D3, D2, NC, 128);
 HIDScope scope(3);
 Ticker ScopeTime;
+Ticker myControllerTicker;
 
 double Aantal_Degs;
 double Aantal_pulses;
-double Error;
-double refference;
-const double Kp = 0.007;
-
 
 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
 {
@@ -31,62 +28,43 @@
     scope.send();
     
 }
+// Controller gain
+const double motor1_Kp = 0.05;
+// Reusable P controller
+double P( double error, const double Kp ) 
+{
+    return Kp * error;
+}
 
+// Next task, measure the error and apply the output to the plant
+void motor1_Controller() 
+{
+    double reference = potmeter2.read()*360;
+    double position = Encoder.getPulses()*360/128/131; // Aantal Degs
+    double P2 = P( reference - position, motor1_Kp );
+    motor2speed = abs(P2);
+    if(P2 > 0)
+    {  
+        motor2direction = 0;
+    }
+    else
+    {
+        motor2direction = 1;
+    }
+
+}
 
 int main()
-{
-    motor2direction = 0;
-    motor2speed = 0;
-    led = 1;
+{   
     pc.baud(115200);
-    refference = 0;
     pc.printf("Tot aan loop werkt\n");
+    
     ScopeTime.attach_us(&ScopeSend, 10e4);
-    
-    
-    while (true) 
+    myControllerTicker.attach( &motor1_Controller, 0.01f ); // 100 Hz
+    while(true)
     {
-        
-        char c = pc.getc();   
-        if(c == 'r')
-        {
-            refference = refference + 10;
-            pc.printf("rx \n");
-            Error = refference - Aantal_Degs;
-            while(abs(Error) > 1)
-                {
-                    Error = refference - Aantal_Degs;
-                    motor2speed = Kp*abs(Error);
-                    pc.printf("reffence = %f,error = %f,Aantal degs = %f \n",refference,Error,Aantal_Degs);
-                    if(Error > 0)
-                    {
-                        motor2direction = 0;
-                    }
-                    else
-                    {
-                        motor2direction = 1;
-                    }
-                }
-        }
-        if(c == 'f')
-        {
-            refference = refference - 10;
-            pc.printf("rx \n");
-            Error = refference - Aantal_Degs;
-            while(abs(Error) > 1)
-                {
-                    Error = refference - Aantal_Degs;
-                    motor2speed = Kp*abs(Error);
-                    pc.printf("reffence = %f,error = %f,Aantal degs = %f \n",refference,Error,Aantal_Degs);
-                    if(Error > 0)
-                    {
-                        motor2direction = 0;
-                    }
-                    else
-                    {
-                        motor2direction = 1;
-                    }
-                }
-            }
+       pc.printf("position = %f aantal degs = %f \n",potmeter2.read()*360,Aantal_Degs);
+       wait(0.1f);
     }
+
 }
\ No newline at end of file