Diff: main.cpp

Revision:

39:e5bf4b1293fa

Parent:

38:eaf245d88d84

Child:

40:cac08f589732

--- a/main.cpp	Thu Oct 29 09:34:20 2015 +0000
+++ b/main.cpp	Thu Oct 29 09:45:12 2015 +0000
@@ -1,8 +1,6 @@
 #include "mbed.h"
 #include "QEI.h"
 #include "math.h"
-#include "HIDScope.h"
-
 
 
 // Motor 1 & 2
@@ -25,13 +23,10 @@
 
 Ticker          biquadTicker;
 Ticker          MotorWrite;
-Ticker          Sender;
 Timer           timer;
 Timer           TijdEMGCal;
 
-// Debugging
-Serial          pc(USBTX, USBRX);
-HIDScope        scope(6);
+// LED
 DigitalOut      ledF(LED_RED);
 DigitalOut      ledC(LED_GREEN);
 
@@ -212,6 +207,10 @@
 
 void MotorSet()
 {
+    Pulses = Encoder.getPulses();
+    Rotatie = (Pulses*twopi)/4200;
+    Pulses2 = Encoder2.getPulses();
+    Rotatie2 = (Pulses2*twopi)/4200;
     // Eerst motor 1 (translatie)
     if (OutRange) {
         Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
@@ -255,18 +254,6 @@
     }
     PowerMotor2.write(fabs(v2));
 }
-void Send()
-{
-    Pulses = Encoder.getPulses();
-    Rotatie = (Pulses*twopi)/4200;
-    Pulses2 = Encoder2.getPulses();
-    Rotatie2 = (Pulses2*twopi)/4200;
-    scope.set(0,Rotatie);
-    scope.set(1,y8);
-    scope.set(2,Rotatie2);
-    scope.set(3,y18);
-    scope.send();
-}
 
 void Calibration ()
 {
@@ -298,23 +285,18 @@
 
 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);
-    Sender.attach(Send,1/Fs);
     MotorWrite.attach(MotorSet,1/Fs);
     biquadTicker.attach(myController,1/Fs2);
     PowerServo.period_ms(20);
     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-0.2;
@@ -376,7 +358,6 @@
                 PowerServo.write(0.04);
                 ledF = 1;
                 Fired=Fired+1;
-                pc.printf("Fire = %i", Fired);
                 if (Fired == 4) {
                     wait (1);
                     Home = true;
@@ -391,7 +372,6 @@
         }
 
         while (Home) { //Terugkeren naar vaste positie
-            pc.printf("Home\n");
             OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd.
             OutRange2 = true;
             Goal = margin;