Diff: main.cpp

Revision:

20:473735947e52

Parent:

19:b8d959e02e5d

Child:

21:d0156eadcbfe

--- a/main.cpp	Wed Oct 07 20:22:21 2015 +0000
+++ b/main.cpp	Fri Oct 09 10:20:15 2015 +0000
@@ -3,7 +3,7 @@
 #include "math.h"
 #include "HIDScope.h"
 
-DigitalOut Direction(D4); //1 = CCW - 0 = CW
+DigitalOut Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down
 PwmOut PowerMotor(D5); //van 0 tot 1
 PwmOut PowerServo(D7);
 DigitalIn Button(PTC6);
@@ -13,6 +13,7 @@
 Serial pc(USBTX, USBRX);
 Ticker MotorWrite;
 Ticker Sender;
+Ticker EMG;
 Timer timer;
 HIDScope scope(3);
 
@@ -20,7 +21,9 @@
 double z=0; //initiele waarde potmeter
 const double twopi = 6.2831853071795;
 int Pulses;
-double Rotatie; //aantal graden dat de motor is gedraaid
+const double downlimit = 0.3
+                         const double margin = 0.3
+                                 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;
@@ -36,7 +39,7 @@
 bool Excecute2 = false;
 const double Fs=100;
 
-double n = 3;
+double n = 4.43546; // Aantal rondjes dat ons apparaat maximaal mag draaien
 
 void MotorSet()
 {
@@ -55,6 +58,20 @@
     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;
@@ -62,46 +79,124 @@
     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 ) {
+        while (Excecute ) { //Dit wordt gebruikt voor motor 1
             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
+            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);
-            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.5) {
-                Errori = Errori + Error*1/Fs;
-            } else {
-                Errori = 0;
+            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);
             }
-            pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
-            if (fabs(Error)<=0.0015) {
-                timer.start();
-            } else {
-                timer.stop();
-                timer.reset();
+            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);
             }
-            if (timer.read() >= 5) {
-                Excecute=false;
-                Excecute2 = true;
-                Error = 0;
-                Errori = 0;
-                Errord = 0;
+
+            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);
             }
         }
+        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; // Het doel waar hij naar toe moet
+            Goal = 0.3; // 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;
@@ -120,7 +215,6 @@
             if (timer.read() >= 5) {
                 Excecute=false;
                 Excecute2 = false;
-                Error = 0;
                 Errori = 0;
                 Errord = 0;
             }