Diff: main.cpp

Revision:

21:d0156eadcbfe

Parent:

20:473735947e52

Child:

22:2e1713475f5f

--- a/main.cpp	Fri Oct 09 10:20:15 2015 +0000
+++ b/main.cpp	Mon Oct 12 13:44:24 2015 +0000
@@ -3,7 +3,7 @@
 #include "math.h"
 #include "HIDScope.h"
 
-DigitalOut Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down
+DigitalOut Direction(D4); //1 = CCW - 0 = CW
 PwmOut PowerMotor(D5); //van 0 tot 1
 PwmOut PowerServo(D7);
 DigitalIn Button(PTC6);
@@ -13,7 +13,6 @@
 Serial pc(USBTX, USBRX);
 Ticker MotorWrite;
 Ticker Sender;
-Ticker EMG;
 Timer timer;
 HIDScope scope(3);
 
@@ -21,9 +20,7 @@
 double z=0; //initiele waarde potmeter
 const double twopi = 6.2831853071795;
 int Pulses;
-const double downlimit = 0.3
-                         const double margin = 0.3
-                                 double Rotatie; //aantal graden dat de motor is gedraaid
+double Rotatie; //aantal graden dat de motor is gedraaid
 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;
@@ -39,7 +36,7 @@
 bool Excecute2 = false;
 const double Fs=100;
 
-double n = 4.43546; // Aantal rondjes dat ons apparaat maximaal mag draaien
+double n = 3;
 
 void MotorSet()
 {
@@ -58,20 +55,6 @@
     scope.set(2,Error);
     scope.send();
 }
-void EMGsignal()
-{
-    // Lees het EMG signaal uit
-    //if (Treshold => x) {
-//        v = 1
-//    } else {
-//        v = 0
-//    }
-//        PowerMotor.write(v)
-}
-
-
-
-
 int main()
 {
     upperlimit = n*twopi;
@@ -79,124 +62,46 @@
     PowerMotor.write(0);
     Sender.attach(Send,0.5/Fs);
     MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
-    EMG.attach(EMGsignal,1/Fs);
     while (true) {
         Encoder.reset();
         if (Button == 0) {
             Excecute =! Excecute;
         }
-        while (Excecute ) { //Dit wordt gebruikt voor motor 1
+        while (Excecute ) {
             Pulses = Encoder.getPulses();
-            Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien
-            Rotatieup = fmod(Rotatie,upperlimit); //Aantal radialen draaien binnen het bereik van upperlimit
+            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 ("Rotatie = %f [radialen] \n", Rotatieup);
-            if (Rotatie >= upperlimit) {
-                Goal = upperlimit - margin;
-                Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
-                Errord = (Error-Errorp)/Fs;
-                Errorp = Error;
-                if (fabs(Error) <= 0.5) {
-                    Errori = Errori + Error*1/Fs;
-                } else {
-                    Errori = 0;
-                }
-                pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
+            Goal = 0.5*twopi; //PotMeter.read()*upperlimit; // Het doel waar hij naar toe moet
+            Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
+            Errord = (Error-Errorp)/Fs;
+            Errorp = Error;
+            if (fabs(Error) <= 0.3) {
+                Errori = Errori + Error*1/Fs;
+            } else {
+                Errori = 0;
             }
-            if (Rotatie <= downlimit) {
-                Goal = 0 + margin;
-                Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
-                Errord = (Error-Errorp)/Fs;
-                Errorp = Error;
-                if (fabs(Error) <= 0.5) {
-                    Errori = Errori + Error*1/Fs;
-                } else {
-                    Errori = 0;
-                }
-                pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
+            pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
+            if (fabs(Error)<=0.0015) {
+                timer.start();
+            } else {
+                timer.stop();
+                timer.reset();
             }
-
-            else {
-                // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
-                Goal = z*upperlimit;
-                Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
-                Errord = (Error-Errorp)/Fs;
-                Errorp = Error;
-                if (fabs(Error) <= 0.5) {
-                    Errori = Errori + Error*1/Fs;
-                } else {
-                    Errori = 0;
-                }
-                pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
+            if (timer.read() >= 5) {
+                Excecute=false;
+                Excecute2 = true;
+                Error = 0;
+                Errori = 0;
+                Errord = 0;
             }
         }
-        while (Excecute) { // Dit is voor motor 2, dus naar boven en naar beneden
-            Pulses = Encoder.getPulses();
-            Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien
-            Rotatieup = fmod(Rotatie,upperlimit); //Aantal radialen draaien binnen het bereik van upperlimit
-            pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
-            if (Rotatie >= upperlimit) {
-                Goal = upperlimit - margin;
-                Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
-                Errord = (Error-Errorp)/Fs;
-                Errorp = Error;
-                if (fabs(Error) <= 0.5) {
-                    Errori = Errori + Error*1/Fs;
-                } else {
-                    Errori = 0;
-                }
-                pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
-            }
-            if (Rotatie <= downlimit) {
-                Goal = 0 + margin;
-                Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
-                Errord = (Error-Errorp)/Fs;
-                Errorp = Error;
-                if (fabs(Error) <= 0.5) {
-                    Errori = Errori + Error*1/Fs;
-                } else {
-                    Errori = 0;
-                }
-                pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
-            }
-
-            else {
-                // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
-                Goal = z*upperlimit;
-                Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
-                Errord = (Error-Errorp)/Fs;
-                Errorp = Error;
-                if (fabs(Error) <= 0.5) {
-                    Errori = Errori + Error*1/Fs;
-                } else {
-                    Errori = 0;
-                }
-                pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
-            }
-        }
-
-
-
-// Dit gedeelte moet vervangen worden if fired then Excecut2 = true after waiting 1 second
-        //      if (fabs(Error)<=0.0015) {
-//                timer.start();
-//            } else {
-//                timer.stop();
-//                timer.reset();
-//            }
-//            if (timer.read() >= 5) {
-//                Excecute=false;
-//                Excecute2 = true;
-//                Errori = 0;
-//                Errord = 0;
-//            }
-
-//    }
         while (Excecute2) {
             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 ("Rotatie = %f [radialen] \n", Rotatieup);
-            Goal = 0.3; // Het doel waar hij naar toe moet
+            Goal = 0; // Het doel waar hij naar toe moet
             Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
             Errord = (Error-Errorp)/Fs;
             Errorp = Error;
@@ -215,6 +120,7 @@
             if (timer.read() >= 5) {
                 Excecute=false;
                 Excecute2 = false;
+                Error = 0;
                 Errori = 0;
                 Errord = 0;
             }