Diff: main.cpp

Revision:

23:c97e206cf2a7

Parent:

22:2e1713475f5f

Child:

24:711c7c388e57

--- a/main.cpp	Mon Oct 12 18:23:05 2015 +0000
+++ b/main.cpp	Wed Oct 14 14:55:21 2015 +0000
@@ -5,27 +5,25 @@
 
 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);
+PwmOut          PowerServo(D3);
 DigitalIn       Button(PTC6);
-DigitalIn       Button2(PTC4);
+DigitalIn       Button2(PTA4);
 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;
+Ticker          sampleEMG;
 Timer           timer;
 HIDScope        scope(3);
 
 
-double          emg_value
+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;
@@ -36,8 +34,10 @@
 const double    Ki = 0.2;
 double          v = 0; //snelheid van de motor (0-0.1
 double          upperlimit; //max aantal rotaties
+const double    downlimit = 0.3;
 bool            Excecute = false;
 bool            Home = false;
+bool            OutRange = false;
 const double    Fs=100;
 const double    T1 = 0.33333; // Treshold 1
 const double    T2 = 0.66666; // Treshold 2
@@ -47,30 +47,29 @@
 
 void MotorSet()
 {
-
-    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 (OutRange) {
+        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;
     }
-    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,emg_value);
     scope.send();
 }
@@ -79,7 +78,6 @@
     // Lees het EMG signaal uit
     //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben
     emg_value = PotMeter.read();
-
 }
 
 
@@ -91,71 +89,84 @@
     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);
+    sampleEMG.attach(EMGsample,0.5/Fs);
+    PowerServo.period_ms(20);
     while (true) {
         Encoder.reset();
         if (Button == 0) {
             Excecute =! Excecute;
         }
         while (Excecute ) { //Dit wordt gebruikt voor motor 1
-            if (Rotatie >= upperlimit) {
+            //pc.printf("Rotatie = %f \n", Rotatie);
+            //pc.printf("Out of Range = %i \n", OutRange);
+            if (Rotatie >= upperlimit-margin) {
                 Goal = upperlimit - margin;
+                OutRange = true;
             }
-            if (Rotatie <= downlimit) {
+            if (Rotatie <= downlimit+margin) {
                 Goal = 0 + margin;
+                OutRange = true;
             }
-
-            else {
+            if (Rotatie >= margin && Rotatie <= (upperlimit - margin)) { // Zodra ik hem uit de Range stuur dan
+                OutRange = false;
                 // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
-                if (emg_value => T2 ) {
+                if (emg_value >= T2 ) {
                     Direction = 1;
                     v = 1;
                 }
                 if (emg_value >= T1 && emg_value <= T2) {
                     Direction = 0;
                     v = 1;
+                } 
+                if (emg_value <= T1){
+                    Direction = 0;
+                    v = 0;
+                }
+            }
+        }
+        while (false ) { //Dit wordt gebruikt voor motor 2 MOET NOG OMGEZET WORDEN NAAR MOTOR 2!!!!
+            if (Rotatie >= upperlimit-margin) {
+                Goal = upperlimit - margin;
+                OutRange = true;
+            }
+            if (Rotatie <= downlimit+margin) {
+                Goal = 0 + margin;
+                OutRange = true;
+            }
+            if (Rotatie >= margin && Rotatie <= (upperlimit - margin)) { // Zodra ik hem uit de Range stuur dan
+                OutRange = false;
+                // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
+                if (emg_value >= T2 ) {
+                    Direction = 0;
+                    v = 1;
+                }
+                if (emg_value >= T1 && emg_value <= T2) {
+                    Direction = 1;
+                    v = 1;
                 } else {
                     Direction = 0;
                     v = 0;
                 }
             }
-            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;
-            }
         }
 
+        if (Button2 == 0) {
+            PowerServo.write(0.27);
+            wait (1);
+            PowerServo.write(0.04);
+            wait (1);
+            Fire=Fire+1;
+        }
+        if (Fire == 3) {
+            wait (1);
+            Excecute = false;
+            Home = true;
+        }
+
+
         while (Home) {
-            Goal = 0.3; // Het doel waar hij naar toe moet
+            OutRange = false;
+            Goal = margin; // Het doel waar hij naar toe moet
             if (fabs(Error)<=0.0015) {
                 timer.start();
             } else {