Diff: main.cpp

Revision:

38:eaf245d88d84

Parent:

37:0dbf536a95c8

Child:

39:e5bf4b1293fa

--- a/main.cpp	Wed Oct 28 14:22:23 2015 +0000
+++ b/main.cpp	Thu Oct 29 09:34:20 2015 +0000
@@ -15,8 +15,8 @@
 PwmOut          PowerServo(D3);
 
 // Buttons & EMG
-DigitalIn       Button(PTC6);
-DigitalIn       Button2(PTA4);
+AnalogIn        Button(A3); // Calibration Button  
+AnalogIn        Button2(A4); // Fire button
 DigitalIn       Button3(D9);
 AnalogIn        EMG(A0);
 AnalogIn        EMG2(A1);
@@ -32,7 +32,8 @@
 // Debugging
 Serial          pc(USBTX, USBRX);
 HIDScope        scope(6);
-DigitalOut      led(LED_RED);
+DigitalOut      ledF(LED_RED);
+DigitalOut      ledC(LED_GREEN);
 
 
 
@@ -48,7 +49,7 @@
 double          T4 = 0;
 
 // Motor 1 (Translatie)
-const double    n1 = 3.861464193; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog)
+const double    n1 = 3.3; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog)
 int             Pulses;
 double          Rotatie = 0; //aantal graden dat de motor is gedraaid
 double          Goal; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
@@ -66,7 +67,7 @@
 bool            OutRange = false;
 
 // Motor 2 (Rotatie)
-const double    n2 = 0.3125; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie)
+const double    n2 = 0.4; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie)
 int             Pulses2;
 double          Rotatie2 = 0;
 double          Goal2;
@@ -270,18 +271,19 @@
 void Calibration ()
 {
     if (OutRange == false && OutRange2 == false) {
-        if (Button == 0) {
+        if (Button >= 0.8) {
+            wait (0.2);
             Cali = true;
             TijdEMGCal.start();
             Excecute = false;
-            led = 0;
+            ledC = 0;
             v = 0;
             v2 = 0;
         }
     }
     if (TijdEMGCal.read() >= TijdCal) {
         Cali = false;
-        led = 1;
+        ledC = 1;
         TijdEMGCal.stop();
         TijdEMGCal.reset();
         T1 = 0.35*Max;
@@ -296,8 +298,11 @@
 
 int main()
 {
+    pc.printf("Welkom\n");
     upperlimit = n1*twopi;
     turnlimit = n2*twopi;
+    ledF = 1;
+    ledC = 1;
     pc.baud(115200);
     PowerMotor.write(0);
     PowerMotor2.write(0);
@@ -305,18 +310,18 @@
     MotorWrite.attach(MotorSet,1/Fs);
     biquadTicker.attach(myController,1/Fs2);
     PowerServo.period_ms(20);
-    led = 1;
     while (true) {
+        pc.printf("grote While Loop\n");
         Calibration();
         while (Excecute) {
             pc.printf("Rotatie = %f \nRotatie2 = %f \n", Rotatie, Rotatie2);
             // Eerst wordt motor 1 aangestuurd
             if (Rotatie >= upperlimit) { //Als hij buiten bereik is
-                Goal = upperlimit - margin;
+                Goal = upperlimit - margin-0.2;
                 OutRange = true;
             }
             if (Rotatie <= downlimit) { //Als hij buiten bereik is
-                Goal = 0 + margin;
+                Goal = 0 + margin +0.2;
                 OutRange = true;
             }
             if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is
@@ -325,11 +330,11 @@
             if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing
                 if (y8 >= T2 ) {
                     Direction = 1;
-                    v = 0.12;
+                    v = 0.13;
                 }
                 if (y8 > T1 && y8 < T2) {
                     Direction = 0;
-                    v = 0.08;
+                    v = 0.07;
                 }
                 if (y8 <= T1) {
                     Direction = 0;
@@ -339,11 +344,11 @@
 
             // Vanaf hier wordt motor 2 aangestuurd
             if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is
-                Goal2 = turnlimit;
+                Goal2 = turnlimit-0.1;
                 OutRange2 = true;
             }
             if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is
-                Goal2 = -turnlimit;
+                Goal2 = -turnlimit+0.1;
                 OutRange2 = true;
             }
             if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is
@@ -352,21 +357,24 @@
             if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
                 if (y18 >= T4 ) {
                     Direction2 = 1;
-                    v2 = 0.08;
+                    v2 = 0.07;
                 }
                 if (y18 > T3 && y18 < T4) {
                     Direction2 = 0;
-                    v2 = 0.08;
+                    v2 = 0.07;
                 }
                 if (y18 <= T3) {
                     Direction2 = 0;
                     v2 = 0;
                 }
             }
-            if (Button2 == 0) { //Afvuren van de RBG
+            if (Button2 >= 0.8) { //Afvuren van de RBG
+                wait(0.2);
+                ledF = 0;
                 PowerServo.write(0.27);
                 wait (1);
                 PowerServo.write(0.04);
+                ledF = 1;
                 Fired=Fired+1;
                 pc.printf("Fire = %i", Fired);
                 if (Fired == 4) {
@@ -402,7 +410,7 @@
                 Errord2 = 0;
                 Errorp2 = 0;
                 Fired = 0;
-                wait (3);
+                wait (5);
                 Excecute = true;
                 Home = false;
             }