Diff: main.cpp

Revision:

25:230bd4e1f3ef

Parent:

24:711c7c388e57

Child:

26:5b6c577fb3c1

--- a/main.cpp	Wed Oct 14 15:27:45 2015 +0000
+++ b/main.cpp	Thu Oct 15 14:24:49 2015 +0000
@@ -3,28 +3,53 @@
 #include "math.h"
 #include "HIDScope.h"
 
+
+
+// Motor 1 & 2
 DigitalOut      Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down
 PwmOut          PowerMotor(D5); //van 0 tot 1
+QEI             Encoder(D10,D11,NC,32,QEI::X2_ENCODING); //Encoder
+DigitalOut      Direction2(D6);
+PwmOut          PowerMotor2(D7);
+QEI             Encoder2(D12,D13,NC,32,QEI::X2_ENCODING);
 PwmOut          PowerServo(D3);
+
+// Buttons & EMG (PotMeter)
 DigitalIn       Button(PTC6);
 DigitalIn       Button2(PTA4);
 AnalogIn        PotMeter(A1);
-AnalogIn        EMG(A0);
-QEI             Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
-Serial          pc(USBTX, USBRX);
+AnalogIn        PotMeter2(A2);
+//AnalogIn        EMG(A0);
+//AnalogIn        Emg(A1);
+
+// Tickers & timers
 Ticker          MotorWrite;
 Ticker          Sender;
 Ticker          sampleEMG;
 Timer           timer;
+
+// Debugging
+Serial          pc(USBTX, USBRX);
 HIDScope        scope(3);
 
 
+
+// Waardes
+const double    twopi = 6.2831853071795;
+const double    Fs=100;
+int             Fired = 0;
+
+// EMG
 double          emg_value;
-const double    twopi = 6.2831853071795;
+double          emg_value2;
+const double    T1 = 0.33333; // Treshold 1
+const double    T2 = 0.66666; // Treshold 2
+
+// Motor 1 (Translatie)
+double          n1 = 3.861464193; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog)
 int             Pulses;
-const double    margin = 0.4;
 double          Rotatie = 0; //aantal graden dat de motor is gedraaid
-double          Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
+double          Goal; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
 double          Error = 0;
 double          Errord = 0;
 double          Errori = 0;
@@ -32,18 +57,37 @@
 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
-const double    downlimit = 0.3;
+double          v = 0; //snelheid van de motor (0-0.1)?
+double          upperlimit; //max aantal rotaties omhoog
+const double    downlimit = 0.4;
+const double    margin = 0.4;
+bool            OutRange = false;
+
+// Motor 2 (Rotatie)
+double          n2 = 0.3125; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie)
+
+int             Pulses2;
+double          Rotatie2 = 0;
+double          Goal2;
+double          Error2 = 0;
+double          Errord2 = 0;
+double          Errori2 = 0;
+double          Errorp2 = 0;
+const double    Kp2 = 0.2;
+const double    Kd2 = 10;
+const double    Ki2 = 0.2;
+double          v2 = 0;
+double          turnlimit; // max aantal rotaties voor roteren hele robot
+// Margin 2 is in ons geval 0
+bool            OutRange2 = false;
+
+// Activatie tussen het schietgedeelte en terugkeergedeelte
 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
-int             Fire = 0;
+
 
-double n = 4.43546; // Aantal rondjes dat ons apparaat maximaal mag draaien
+
+// Voids voor berekeningen in het hoofdprogramma
 
 void MotorSet()
 {
@@ -51,7 +95,7 @@
         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) {
+        if (fabs(Error) <= 0.1) {
             Errori = Errori + Error*1/Fs;
         } else {
             Errori = 0;
@@ -64,11 +108,31 @@
         v=Kp*Error + Kd*Errord + Ki*Errori;
     }
     PowerMotor.write(fabs(v));
+
+    if (OutRange2) {
+        Error2 = Goal2-Rotatie2; // De error die het motortje maakt ten opzichte van je Goal
+        Errord2 = (Error2-Errorp2)/Fs;
+        Errorp2 = Error2;
+        if (fabs(Error2) <= 0.5) {
+            Errori2 = Errori2 + Error2*1/Fs;
+        } else {
+            Errori2 = 0;
+        }
+        if (Error2>=0) {
+            Direction=1;
+        } else {
+            Direction=0;
+        }
+        v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2;
+    }
+    PowerMotor2.write(fabs(v2));
 }
 void Send()
 {
     Pulses = Encoder.getPulses();
     Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien
+    Pulses2 = Encoder2.getPulses();
+    Rotatie2 = (Pulses*twopi)/4200;
     scope.set(0,Goal);
     scope.set(1,Rotatie);
     scope.set(2,emg_value);
@@ -79,13 +143,32 @@
     // Lees het EMG signaal uit
     //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben
     emg_value = PotMeter.read();
+    emg_value2 = PotMeter2.read();
 }
 
+void Fire ()
+{
+    PowerServo.write(0.27);
+    wait (1);
+    PowerServo.write(0.04);
+    wait (1);
+    Fired=Fired+1;
+    if (Fired >= 3) {
+        wait (1);
+        Excecute = false;
+        Home = true;
+        Fired = 0;
+    }
+}
+
+
+// Calibratie moet nog worden uitgevoerd!!!!
 
 
 int main()
 {
-    upperlimit = n*twopi;
+    upperlimit = n1*twopi;
+    turnlimit = n2*twopi;
     pc.baud(115200);
     PowerMotor.write(0);
     Sender.attach(Send,0.5/Fs);
@@ -94,25 +177,25 @@
     PowerServo.period_ms(20);
     while (true) {
         Encoder.reset();
+        Encoder2.reset();
         if (Button == 0) {
             Excecute =! Excecute;
         }
-        while (Excecute ) { //Dit wordt gebruikt voor motor 1
-            //pc.printf("Rotatie = %f \n", Rotatie);
-            //pc.printf("Out of Range = %i \n", OutRange);
-            if (Rotatie >= upperlimit) {
+
+        while (Excecute ) {
+            // Eerst wordt motor 1 aangestuurd
+            if (Rotatie >= upperlimit) { //Als hij buiten bereik is
                 Goal = upperlimit - margin;
                 OutRange = true;
             }
-            if (Rotatie <= downlimit) {
+            if (Rotatie <= downlimit) { //Als hij buiten bereik is
                 Goal = 0 + margin;
                 OutRange = true;
             }
-            if (Rotatie >= margin && Rotatie <= upperlimit - margin) {
+            if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is
                 OutRange = false;
             }
-            if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) {
-                // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
+            if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing
                 if (emg_value >= T2 ) {
                     Direction = 1;
                     v = 1;
@@ -126,62 +209,59 @@
                     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;
+            // Vanaf hier wordt motor 2 aangestuurd
+            if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is
+                Goal2 = turnlimit;
+                OutRange2 = true;
+            }
+            if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is
+                Goal2 = -turnlimit;
+                OutRange2 = true;
             }
-            if (Rotatie <= downlimit+margin) {
-                Goal = 0 + margin;
-                OutRange = true;
+            if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is
+                OutRange2 = false;
             }
-            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 (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
+
+                if (emg_value2 >= T2 ) {
+                    Direction = 1;
+                    v = 1;
+                }
+                if (emg_value2 >= T1 && emg_value2 <= T2) {
                     Direction = 0;
                     v = 1;
                 }
-                if (emg_value >= T1 && emg_value <= T2) {
-                    Direction = 1;
-                    v = 1;
-                } else {
+                if (emg_value2 <= T1) {
                     Direction = 0;
                     v = 0;
                 }
             }
+            if (Button2 == 0) { //Afvuren van de RBG
+                Fire();
+            }
         }
 
-        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) {
-            OutRange = false;
-            Goal = margin; // Het doel waar hij naar toe moet
-            if (fabs(Error)<=0.0015) {
+        while (Home) { //Terugkeren naar vaste positie
+            OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd.
+            Goal = 0;
+            Goal2 = 0;
+            if (fabs(Error)<=0.0015 && fabs(Error2)<=0.0015) {
                 timer.start();
             } else {
                 timer.stop();
                 timer.reset();
             }
             if (timer.read() >= 3) {
-                Excecute=false;
                 Home = false;
                 Errori = 0;
                 Errord = 0;
+                Errorp = 0;
+                Errori2 = 0;
+                Errord2 = 0;
+                Errorp2 = 0;
+                wait (10);
+                Excecute = true;
             }
         }
     }