Diff: main.cpp

Revision:

14:b9c925a8176a

Parent:

13:c60942a1da8e

Child:

15:f7e2b20f4dca

--- a/main.cpp	Wed Oct 07 13:30:46 2015 +0000
+++ b/main.cpp	Wed Oct 07 14:48:06 2015 +0000
@@ -9,7 +9,7 @@
 AnalogIn PotMeter(A1);
 Encoder MotorEncoder(D12,D13); //Encoder
 Serial pc(USBTX, USBRX);
-Ticker Pot;
+Ticker MotorWrite;
 HIDScope scope(3);
 
 double z=0; //initiele waarde potmeter
@@ -17,19 +17,26 @@
 const double pi = twopi/2;
 int Pulses;
 double Rotatie; //aantal graden dat de motor is gedraaid
-double Rotatieup; //aantal graden dat de motor is gedraaid in een bereik van n*pi
+double Rotatieup=0; //aantal graden dat de motor is gedraaid in een bereik van n*pi
 double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
 double Error = 0;
 double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
 double v = 0; //snelheid van de motor (0-0.1
 double upperlimit; //max aantal rotaties
 bool Excecute = false;
+double Fs=100;
 
 double n = 8;
 
-void readpot()
+void MotorSet()
 {
-    z = PotMeter.read(); //uitlezen Potmeter of voor EMG bijvoorbeeld
+    v=Kp*Error;
+    if (v>=0) {
+        Direction=1;
+    } else {
+        Direction=0;
+    }
+    PowerMotor.write(fabs(v));
 }
 
 int main()
@@ -37,7 +44,7 @@
     upperlimit = n*twopi;
     pc.baud(115200);
     PowerMotor.write(0);
-    Pot.attach(readpot,0.1); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde
+    MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde
     while (true) {
         if (Button == 0) {
             Excecute =! Excecute;
@@ -48,20 +55,13 @@
             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
+            Goal = PotMeter.read()*upperlimit; // Het doel waar hij naar toe moet
             Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
-            if (Error >= 0) { //Bepalen van de rotatie richting
-                Direction =1;
-            } else {
-                Direction =0;
+            pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
+            if (Error>=-0.1 && Error<=0.1) {
+                Excecute=false;
+                PowerMotor.write(0);
             }
-            pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
-            v = Kp*fabs(Error); // 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.set(2,Error);