Diff: main.cpp

Revision:

37:af85a7b57a25

Parent:

36:344588e69589

--- a/main.cpp	Wed Oct 26 10:04:38 2016 +0000
+++ b/main.cpp	Wed Oct 26 12:20:24 2016 +0000
@@ -3,7 +3,6 @@
 #include "BiQuad.h"
 #include "MODSERIAL.h"
 
-MODSERIAL pc(USBTX, USBRX);
 
 //Define objects
 //Define the button interrupt for the calibration
@@ -19,6 +18,7 @@
 Ticker      sample_timer;
 
 HIDScope    scope( 6 );
+MODSERIAL pc(USBTX, USBRX);
 
 //Initialize all variables
 volatile bool sampletimer = false;
@@ -30,13 +30,20 @@
 double emg02;
 double emg12;
 double emg22;
-double ref_x=0.000;
-double ref_y=0.000;
+double ref_x=0.0000;
+double ref_y=0.0000;
+double old_ref_x;
+double old_ref_y;
 double speed_emg=0.00002;
 double speed_key=0.000326;
-double speed;
+double speed=0.00002;
+double theta=0.0;
+double radius=0.0;
 
-const int negative=-1;
+const double minRadius=0.3;                // minimum radius of arm
+const double maxRadius=0.6;                // maximum radius of arm
+const double minAngle=-1.25;               // minimum angle for limiting controller
+
 char key;
 
 
@@ -91,6 +98,9 @@
 
 void sampleflag()
 {
+     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
     sampletimer=true;
 }
@@ -98,8 +108,8 @@
 void calibrate()
 {
     //This resets the reference signals so that the robot can be calibrated
-    ref_x=0.0;
-    ref_y=0.0;
+    ref_x=0.0000;
+    ref_y=0.0000;
 }
 
 void sample(states &mystate)
@@ -108,12 +118,11 @@
 
     //This checks if a key is pressed and adjusts the speed to the needed speed
     if (pc.readable()==1) {
+        
         key=pc.getc();
-        speed=speed_key;
-    } else {
-        key ='p';
-        speed=speed_emg;
-    }
+        //printf("%c\n\r",key);
+    } 
+
 
     //Read the emg signals and filter it
 
@@ -127,23 +136,26 @@
     emg22=bqc33.step(fabs(bqc31.step(emg3.read())));    //filtered signal 2
     scope.set(5, emg22);
 
+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();
 
+    old_ref_x=ref_x;
+    old_ref_y=ref_y;
     //look if the emg signals go over the threshold and change the state to the cooresponding state. Also change the reference.
     if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') {
         mystate = STATE_XY_NEG;
-        ref_x=ref_x+speed*negative;
-        ref_y=ref_y+speed*negative;
+        ref_x=ref_x-speed;
+        ref_y=ref_y-speed;
 
     } else if (emg02>threshold&&emg12>threshold || key == 'a' ) {
         mystate = STATE_X_NEG;
-        ref_x=ref_x+speed*negative;
+        ref_x=ref_x-speed;
 
     } else if (emg02>threshold&&emg22>threshold || key == 's') {
         mystate = STATE_Y_NEG;
-        ref_y=ref_y+speed*negative;
+        ref_y=ref_y-speed;
 
     } else if (emg12>threshold&&emg22>threshold || key == 'e' ) {
         mystate = STATE_XY;
@@ -158,7 +170,31 @@
         mystate = STATE_Y;
         ref_y=ref_y+speed;
     } else {
-        //mystate = STATE_PAUZE;
+        mystate = STATE_PAUZE;
+    }
+
+    // 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) {
+        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 ) {
+        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
@@ -173,7 +209,7 @@
 void my_pos()
 {
     //This function is attached to a ticker so that the reference position is printed every second.
-    pc.printf("x_pos=%.4f\ty_pos=%.4f\n\r",ref_x,ref_y);
+    pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta);
 
 }