Werkend aansturingsscript voor 2 motoren, incl werkende program switch. Motoren oscilleren nog iets. Vuur mechanisme ontbreekt nog.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of frdm_Motor_V2_2 by Margreeth de Breij

Files at this revision

API Documentation at this revision

Comitter:
Rvs94
Date:
Tue Sep 29 13:25:09 2015 +0000
Parent:
8:69bde5e32dbf
Child:
10:80fe931a71e4
Commit message:
Werkend, functies toegevoegd net als in college, aangestuurd door potmeter. Kp waarde is redelijk accuraat.

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
--- 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