Diff: main.cpp

Revision:

39:5db36ce5e620

Parent:

35:6110d0b5513b

Child:

40:e217056584be

diff -r 6110d0b5513b -r 5db36ce5e620 main.cpp
--- a/main.cpp	Tue Oct 30 09:11:19 2018 +0000
+++ b/main.cpp	Wed Oct 31 11:06:29 2018 +0000
@@ -258,8 +258,7 @@
         case operational:       //interpreting emg-signals to move the end effector
             if (state_changed == true){
                     state_changed = false;
-                }
-            
+            }
                        
             // here we have to determine the desired velocity based on the processed emg signals and calibration
             if (process_emg_0 >= 0.16) { des_vx = vxmax; }
@@ -279,7 +278,7 @@
                 
                 make_button_active.attach(&unsuppressing_button,0.5);
 
-                } 
+            } 
             
             break;
             
@@ -287,7 +286,28 @@
             // We want to draw a square. Hence, first move to a certain point and then start moving a square.
             if (state_changed == true){
                     state_changed = false;
-                }
+                    state_timer.reset();
+                    state_timer.start();
+                    des_vx = 0.1;           // first we move to the right
+                    des_vy = 0.0;
+            }
+            static double new_vx, new_vy;
+                        
+            if(state_timer > 3.0f){         // after waiting an extra second we start on another side of the square
+                state_timer.reset();
+                state_timer.start();
+                des_vx = new_vx;
+                des_vy = new_vy;
+            }
+            else if(state_timer > 2.0f){                          // we have a velocity of 0.1 m/s for 2 seconds, so we make a square of 20 by 20 cm
+                if(des_vx > 0){new_vx = 0.0; new_vy = 0.1;}       // here we check which side of the square we were on
+                else if(des_vy > 0){new_vx = -0.1; new_vy = 0.0;}
+                else if(des_vx < 0){new_vx = 0.0; new_vy = -0.1;}
+                else if(des_vy < 0){new_vx = 0.1; new_vy = 0.0;}
+                des_vx = 0;
+                des_vy = 0;
+            }
+       
                 
             if (button.read() == true && button_suppressed == false ) {
                 current_state = operational;