Diff: main.cpp

Revision:

28:b7d01a55530f

Parent:

27:9cca2ad74ec0

Child:

29:7653adbbb101

--- a/main.cpp	Thu Oct 15 16:03:40 2015 +0000
+++ b/main.cpp	Fri Oct 16 09:27:42 2015 +0000
@@ -9,15 +9,15 @@
 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);
+DigitalOut      Direction2(D7);
+PwmOut          PowerMotor2(D6);
+QEI             BIER(D12,D13,NC,32,QEI::X2_ENCODING);
 PwmOut          PowerServo(D3);
 
 // Buttons & EMG (PotMeter)
 DigitalIn       Button(PTC6);
 DigitalIn       Button2(PTA4);
-AnalogIn        PotMeter(A1);
+AnalogIn        PotMeter(A0);
 AnalogIn        PotMeter2(A2);
 //AnalogIn        EMG(A0);
 //AnalogIn        Emg(A1);
@@ -76,7 +76,7 @@
 const double    Kd2 = 10;
 const double    Ki2 = 0.2;
 double          v2 = 0;
-double          turnlimit; // max aantal rotaties voor roteren hele robot
+double          turnlimit = 0.4; // max aantal rotaties voor roteren hele robot
 // Margin 2 is in ons geval 0
 bool            OutRange2 = false;
 
@@ -120,9 +120,9 @@
             Errori2 = 0;
         }
         if (Error2>=0) {
-            Direction=1;
+            Direction2 = 1;
         } else {
-            Direction=0;
+            Direction2 = 0;
         }
         v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2;
     }
@@ -132,7 +132,7 @@
 {
     Pulses = Encoder.getPulses();
     Rotatie = (Pulses*twopi)/4200; 
-    Pulses2 = Encoder2.getPulses();
+    Pulses2 = BIER.getPulses();
     Rotatie2 = (Pulses2*twopi)/4200;
     scope.set(0,Goal);
     scope.set(1,Rotatie);
@@ -172,13 +172,14 @@
     turnlimit = n2*twopi;
     pc.baud(115200);
     PowerMotor.write(0);
+    PowerMotor2.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
     sampleEMG.attach(EMGsample,0.5/Fs);
     PowerServo.period_ms(20);
     while (true) {
         Encoder.reset();
-        Encoder2.reset();
+        BIER.reset();
         if (Button == 0) {
             Excecute =! Excecute;
         }
@@ -200,11 +201,11 @@
             if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing
                 if (emg_value >= T2 ) {
                     Direction = 1;
-                    v = 1;
+                    v = 0.1;
                 }
-                if (emg_value >= T1 && emg_value <= T2) {
+                if (emg_value > T1 && emg_value < T2) {
                     Direction = 0;
-                    v = 1;
+                    v = 0.1;
                 }
                 if (emg_value <= T1) {
                     Direction = 0;
@@ -212,33 +213,32 @@
                 }
             }
 
-            //// 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 (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is
-//                OutRange2 = false;
-//            }
-//            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_value2 <= T1) {
-//                    Direction = 0;
-//                    v = 0;
-//                }
-//            }
+            // 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 (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is
+                OutRange2 = false;
+            }
+            if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
+                if (emg_value2 >= T2 ) {
+                    Direction2 = 1;
+                    v2 = 0.1;
+                }
+                if (emg_value2 > T1 && emg_value2 < T2) {
+                    Direction2 = 0;
+                    v2 = 0.1;
+                }
+                if (emg_value2 <= T1) {
+                    Direction2 = 0;
+                    v2 = 0;
+                }
+            }
             if (Button2 == 0) { //Afvuren van de RBG
                 Fire();
             }