Diff: main.cpp

Revision:

38:0c8a6b0834da

Parent:

37:af85a7b57a25

Child:

39:955929e25858

--- a/main.cpp	Wed Oct 26 12:20:24 2016 +0000
+++ b/main.cpp	Wed Oct 26 13:00:12 2016 +0000
@@ -98,7 +98,7 @@
 
 void sampleflag()
 {
-     if (sampletimer==true) {
+    if (sampletimer==true) {
         pc.printf("rate too high error in setgoflag\n\r");
     }
     //This sets the go flag for when the function sample needs to be called
@@ -118,10 +118,10 @@
 
     //This checks if a key is pressed and adjusts the speed to the needed speed
     if (pc.readable()==1) {
-        
+
         key=pc.getc();
         //printf("%c\n\r",key);
-    } 
+    }
 
 
     //Read the emg signals and filter it
@@ -136,7 +136,7 @@
     emg22=bqc33.step(fabs(bqc31.step(emg3.read())));    //filtered signal 2
     scope.set(5, emg22);
 
-emg02=1;
+    emg02=1;
     //Ensure that enough channels are available at the top (HIDScope scope( 6 ))
     //Finally, send all channels to the PC at once
     scope.send();
@@ -174,27 +174,18 @@
     }
 
     // convert ref to gearbox angle
-    theta=atan((ref_y+sin(theta)*minRadius)/(ref_x+cos(theta)*minRadius));
-    radius=sqrt(pow(ref_x+cos(theta)*minRadius,2)+pow(ref_y+sin(theta)*minRadius,2));
-    if (theta != theta) {
-        theta=0;
-    }
-    if (theta <= minAngle) {
+    theta=atan(ref_y/(ref_x+minRadius));
+    radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
+
+    if (theta < minAngle) {
         ref_x=old_ref_x;
         ref_y=old_ref_y;
-        pc.printf("fout 1 ");
     } else if (radius < minRadius) {
         ref_x=old_ref_x;
         ref_y=old_ref_y;
-        pc.printf("fout 2 ");
-    } /*else if (theta >= 0 ) {
+    } else if ( radius > maxRadius) {
         ref_x=old_ref_x;
         ref_y=old_ref_y;
-        pc.printf("fout 3 ");
-    }*/ else if ( radius > maxRadius) {
-        ref_x=old_ref_x;
-        ref_y=old_ref_y;
-        pc.printf("fout 4 ");
     }
 
     //change newcase so that the state will only be printed once