Diff: main.cpp

Revision:

30:176ca1193a0a

Parent:

29:263c680068db

Child:

31:113f630f7e7d

--- a/main.cpp	Sat Oct 10 22:00:24 2015 +0000
+++ b/main.cpp	Mon Oct 12 20:41:07 2015 +0000
@@ -7,7 +7,6 @@
 
 MODSERIAL   pc(USBTX,USBRX);
 
-
     //  (DEBUGGING AND TESTING Tools) (0 when pressed and 1 when not pressed)   //
 DigitalIn buttonL1(PTC6);                // Button 1 (laag op board) for testing at the lower board
 DigitalIn buttonL2(PTA4);                // Button 2 (laag op board) for testing at the lower board
@@ -51,16 +50,15 @@
     double reference_turn=0;                  // Set constant to store reference value of the Turn motor
     double position_turn;                     // Set constant to store current position of the Turn motor
     double position_strike;
-    double EMG_R;
-    double EMG_L;
+    float EMG_R;
+    float EMG_L;
     double n;
-    
     debug_led_red=off;
     debug_led_blue=off;
     debug_led_green=off;
     
     //  START OF CODE    //
-    pc.printf("Start of code \n\r");
+    pc.printf(" Start of code \n\r");
     
     pc.baud(115200);                                    // Set the baudrate
     
@@ -76,25 +74,30 @@
         const double Threshold_Bicep_Right_1=((max_R_EMG-min_R_EMG)*0.2)+min_R_EMG;     //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT
         const double Threshold_Bicep_Right_2=((max_R_EMG-min_R_EMG)*0.6)+min_R_EMG;     //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
         const double change_one_bottle=45; //(45 graden change)
+        
+        pc.printf("left 1: %d left 2: %d right 1: %d right 2: %d  \n\r", Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, Threshold_Bicep_Right_1, Threshold_Bicep_Right_2);
     
-    // Tickers 
+    // Tickers  // 
     Hidscope_measure.attach(send, sample_time);       // GAAT NOG NIET GOED WAARSCHIJNLIJK ALS EEN WHILE LOOP WORDT UTIGEVOERD
     
     pc.printf("wait \n\r");
     wait (3);                                                                          // Wait before starting system
     pc.printf("start control \n\r");
     
-    //INFINITE LOOP
+    //INFINITE LOOP //
     while(1) 
         {                                                                              // Start while loop  (ACTION loop)    
         // INPUT Filtered EMG SIGNAAL
-        EMG_R = (potmeter1.read())*100;                                                //EMG Right bicep (tussen nul en 100%)
-        EMG_L = (potmeter2.read())*100;                                                // EMG Left bicep  (tussen nul en 100%)
         
         Nieuwe_actie:                                                                  // start here again when action has finished
         debug_led_red=off;
         debug_led_blue=off;
         debug_led_green=on;                                                            // LED Turns green if ready for a new action
+        wait(1);
+        EMG_R = (potmeter1.read());                                                //EMG Right bicep (tussen nul en 100%)
+        EMG_L = (potmeter2.read());                                                // EMG Left bicep  (tussen nul en 100%)
+        pc.printf("EMG_L = %f EMG_R = %f \n\r", EMG_L, EMG_R);
+        
         
         // SLAG                                                                        // (No LED -> 0.55s -> red on (turns on every time a new pwm is given) -> finish)
         if (EMG_L > Threshold_Bicep_Left_1 || EMG_R > Threshold_Bicep_Right_1)
@@ -113,10 +116,10 @@
                             n=1;
                         }     
                     debug_led_red=off;
-                    wait(0.05);                                                        // wait 10 samples
+                    wait(0.10);                                                        // wait 20 samples
                     debug_led_red=on;
                     pwm_motor_strike=((EMG_L-min_L_EMG)+(EMG_R-min_R_EMG))/((max_L_EMG-min_L_EMG)+(max_R_EMG-min_R_EMG))*0.7 + 0.3;     // min speed 0.3 en max 1
-                    wait(0.05);                                                        // wait 10 samples more (pwm changes per 0.1 seconds)
+                    wait(0.10);                                                        // wait 20 samples more (pwm changes per 0.1 seconds)
                     motordirection_strike=cw;                                          // towards bottle
                     
                     if((position_strike-Hit)<2)                                        // bottle is hit when position-hit <2 defrees