Diff: main.cpp

Revision:

22:2e1713475f5f

Parent:

21:d0156eadcbfe

Child:

23:c97e206cf2a7

--- a/main.cpp	Mon Oct 12 13:44:24 2015 +0000
+++ b/main.cpp	Mon Oct 12 18:23:05 2015 +0000
@@ -3,58 +3,87 @@
 #include "math.h"
 #include "HIDScope.h"
 
-DigitalOut Direction(D4); //1 = CCW - 0 = CW
-PwmOut PowerMotor(D5); //van 0 tot 1
-PwmOut PowerServo(D7);
-DigitalIn Button(PTC6);
-DigitalIn Button2(PTC4);
-AnalogIn PotMeter(A1);
-QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
-Serial pc(USBTX, USBRX);
-Ticker MotorWrite;
-Ticker Sender;
-Timer timer;
-HIDScope scope(3);
+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);
+DigitalIn       Button2(PTC4);
+AnalogIn        PotMeter(A1);
+AnalogIn        EMG(A0);
+QEI             Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
+Serial          pc(USBTX, USBRX);
+Ticker          MotorWrite;
+Ticker          Sender;
+Ticker          EMG;
+Timer           timer;
+HIDScope        scope(3);
 
 
-double z=0; //initiele waarde potmeter
-const double twopi = 6.2831853071795;
-int Pulses;
-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;
-double Errord = 0;
-double Errori = 0;
-double Errorp = 0;
-const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
-const double Kd = 10;
-const double Ki = 0.2;
-double v = 0; //snelheid van de motor (0-0.1
-double upperlimit; //max aantal rotaties
-bool Excecute = false;
-bool Excecute2 = false;
-const double Fs=100;
+double          emg_value
+const double    twopi = 6.2831853071795;
+int             Pulses;
+const double    downlimit = 0.3;
+const double    margin = 0.3;
+double          Rotatie = 0; //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;
+double          Errord = 0;
+double          Errori = 0;
+double          Errorp = 0;
+const double    Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
+const double    Kd = 10;
+const double    Ki = 0.2;
+double          v = 0; //snelheid van de motor (0-0.1
+double          upperlimit; //max aantal rotaties
+bool            Excecute = false;
+bool            Home = false;
+const double    Fs=100;
+const double    T1 = 0.33333; // Treshold 1
+const double    T2 = 0.66666; // Treshold 2
+int             Fire = 0;
 
-double n = 3;
+double n = 4.43546; // Aantal rondjes dat ons apparaat maximaal mag draaien
 
 void MotorSet()
 {
-    v=Kp*Error + Kd*Errord + Ki*Errori;
+
+    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;
+    }
     if (Error>=0) {
         Direction=1;
     } else {
         Direction=0;
     }
+    v=Kp*Error + Kd*Errord + Ki*Errori;
     PowerMotor.write(fabs(v));
 }
 void Send()
 {
+    Pulses = Encoder.getPulses();
+    Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien
+    Rotatieup = fmod(Rotatie,upperlimit); //Aantal radialen draaien binnen het bereik van upperlimit
     scope.set(0,Goal);
     scope.set(1,Rotatieup);
-    scope.set(2,Error);
+    scope.set(2,emg_value);
     scope.send();
 }
+void EMGsample()
+{
+    // Lees het EMG signaal uit
+    //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben
+    emg_value = PotMeter.read();
+
+}
+
+
+
 int main()
 {
     upperlimit = n*twopi;
@@ -62,68 +91,83 @@
     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(EMGsample,0.5/Fs);
     while (true) {
         Encoder.reset();
         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 ("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.3) {
-                Errori = Errori + Error*1/Fs;
-            } else {
-                Errori = 0;
+        while (Excecute ) { //Dit wordt gebruikt voor motor 1
+            if (Rotatie >= upperlimit) {
+                Goal = upperlimit - margin;
+            }
+            if (Rotatie <= downlimit) {
+                Goal = 0 + margin;
+            }
+
+            else {
+                // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
+                if (emg_value => T2 ) {
+                    Direction = 1;
+                    v = 1;
+                }
+                if (emg_value >= T1 && emg_value <= T2) {
+                    Direction = 0;
+                    v = 1;
+                } else {
+                    Direction = 0;
+                    v = 0;
+                }
             }
-            pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
+            while (Excecute) { // Dit is voor motor 2, dus naar boven en naar beneden
+                OutofRange = true if
+                if (Rotatie >= upperlimit) {
+                        Goal = upperlimit - margin;
+                    }
+                if (Rotatie <= downlimit) {
+                    Goal = 0 + margin;
+                }
+
+                else {
+                    // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
+                    if (emg_value => T2 ) {
+                        Direction = 1;
+                        v = 1;
+                    }
+                    if (emg_value >= T1 && emg_value <= T2) {
+                        Direction = 0;
+                        v = 1;
+                    } else {
+                        Direction = 0;
+                        v = 0;
+                    }
+                }
+            }
+            if (Button2 = 0) {
+                // servo motor activation --> make a function of it
+                Fire=Fire+1;
+            }
+            if (Fire == 3) {
+                wait (1);
+                Excecute = false;
+                Home = true;
+            }
+        }
+
+        while (Home) {
+            Goal = 0.3; // Het doel waar hij naar toe moet
             if (fabs(Error)<=0.0015) {
                 timer.start();
             } else {
                 timer.stop();
                 timer.reset();
             }
-            if (timer.read() >= 5) {
+            if (timer.read() >= 3) {
                 Excecute=false;
-                Excecute2 = true;
-                Error = 0;
-                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
-            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;
-            }
-            pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
-            if (fabs(Error)<=0.0015) {
-                timer.start();
-            } else {
-                timer.stop();
-                timer.reset();
-            }
-            if (timer.read() >= 5) {
-                Excecute=false;
-                Excecute2 = false;
-                Error = 0;
+                Home = false;
                 Errori = 0;
                 Errord = 0;
             }
         }
     }
-}
\ No newline at end of file
+}