Diff: main.cpp

Revision:

12:dcf90cafb2a5

Parent:

11:a9a23042fc9e

Child:

13:c60942a1da8e

--- a/main.cpp	Mon Sep 28 21:16:40 2015 +0000
+++ b/main.cpp	Wed Sep 30 19:25:46 2015 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "QEI.h"
 #include "math.h"
+#include "HIDScope.h"
 
 DigitalOut Direction(D4); //1 = CCW - 0 = CW
 PwmOut PowerMotor(D5); //van 0 tot 1
@@ -9,6 +10,7 @@
 QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
 Serial pc(USBTX, USBRX);
 Ticker Pot;
+HIDScope scope(2);
 
 double z=0; //initiele waarde potmeter
 const double twopi = 6.2831853071795;
@@ -32,7 +34,6 @@
     z = PotMeter.read(); //uitlezen Potmeter of voor EMG bijvoorbeeld
 }
 
-
 int main()
 {
     upperlimit = n*twopi;
@@ -42,28 +43,31 @@
     while (true) {
         if (Button == 0) {
             Excecute =! Excecute;
+        }
+        while (Excecute ) {
+            Pulses = Encoder.getPulses();
+            Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
+            Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
+            pc.printf ("Potmeter = %f\n", z);
+            pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
+            Goal = z*upperlimit; // Het doel waar hij naar toe moet
+            Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
+            Errorv = 0-v;
+            if (Error >= 0) { //Bepalen van de rotatie richting
+                Direction =0;
+            } else {
+                Direction =1;
             }
-        
-            while (Excecute ) {
-                Pulses = Encoder.getPulses();
-                Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
-                Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
-                pc.printf ("Potmeter = %f\n", z);
-                pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
-                Goal = z*upperlimit; // Het doel waar hij naar toe moet
-                Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
-                Errorv = 0-v;
-                if (Error >= 0) { //Bepalen van de rotatie richting
-                    Direction =0;
-                } else {
-                    Direction =1;
-                }
-                pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
-                v = Kp*fabs(Error)+Kd*fabs(Errorv); // Snelheid van motor
-                PowerMotor.write(v); // Snelheid die naar de motor geschreven wordt (wordt alleen in een schaal van 0-1 weergegeven). (uit ervaring van Motor4_Setpoint loopt dit maar tussen 0-0.1).
+            pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
+            v = Kp*fabs(Error)+Kd*fabs(Errorv); // Snelheid van motor
+            PowerMotor.write(v); // Snelheid die naar de motor geschreven wordt (wordt alleen in een schaal van 0-1 weergegeven). (uit ervaring van Motor4_Setpoint loopt dit maar tussen 0-0.1).
+            if (Error<=0.001 && Error>=-0.001) {
+                PowerMotor.write(0);
+                Excecute=false;
             }
+            scope.set(0,Goal);
+            scope.set(1,Rotatieup);
+            scope.send();
         }
     }
-
-
-
+}
\ No newline at end of file