Diff: main.cpp

Revision:

6:1518fccc5616

Parent:

5:0597358d0016

Child:

7:d55569b92f30

--- a/main.cpp	Mon Oct 26 06:21:49 2015 +0000
+++ b/main.cpp	Mon Oct 26 11:13:37 2015 +0000
@@ -76,12 +76,12 @@
 double          cali_array1[2560]           = {};                           // Array to store values in for the 5 seconds calibartion for y1.
 double          cali_array2[2560]           = {};                           // Array to store values in for the 5 seconds calibartion for y2.
 volatile bool   sample_go;                                                  // Ticker EMG function.
-bool            flag_calibration            = true;                         // Flag to start calibration.
+bool            flag_calibration            = true;                         // Flag to start the calibration.
 
 //**************************** VARIABLES FOR FASE SWITCH ***********************************
 int             fase                        = 3;                            // The fase is used in a switch to switch between the fases. It starts at the reset fase. 
-int             j                           = 1;                            // Starting up a new part of the game. 
-int             N                           = 200;                          // Stop for j.
+int             j                           = 1;                            // Wait time. Used in problem1 and fase switch
+int             N                           = 200;                          // Maximum value of j.
 bool            fase_switch_wait            = true;                         // Timer wait to switch between different fases.
 
 //**************************** VARIABLES FOR CONTROL 2 *************************************
@@ -105,8 +105,8 @@
 const float     minimum_throw_angle         = 20;                           // The minimum angle the arm has to be in to be able to turn the magnet off. 
 const float     maximum_throw_angle         = 110;                          // The maximum angle for the arm to turn the magnet off. 
 float           pos_arm                     = 0;                            // The begin position of the arm. It begins at zero. Reads Encoder1 in degrees. Formula is applied in SensorTicker.
-int             pos_arm1                    = 0;                            // This makes from the positon of the arm degrees within a cirkel.
-float           pos_arm2                    = 0;                            // This makes from the positon of the arm degrees within a cirkel.
+int             pos_arm1                    = 0;                            // This makes from the positon of the arm degrees within a cirkel as an integer.
+float           pos_arm2                    = 0;                            // This makes from the positon of the arm degrees within a cirkel as an float.
 float           previous_pos_arm            = 0;                            // Needed to calculate the v_arm.
 float           v_arm                       = 0;                            // The speed of the arm.
 float           v_arm_com                   = 0;                            // The compensated speed of the arm. 
@@ -115,10 +115,10 @@
 int             switch_rounds               = 2;                            // Value of a switch to calculate the number of rounds made.                          
 int             rounds                      = 0;                            // Number of rounds made by the arm.
 float           pos_armrounds_max           = 3;                            // Max rounds the arm can make.
-bool            problem1                    = false;                        //
-bool            problem2                    = false;                        //
-bool            flag_v_arm                  = false;                        //
-float           problem_velocity            = 0;                            //
+bool            problem1                    = false;                        // Stop for fase 2. It is a wait for problem2 begins.
+bool            problem2                    = false;                        // The reset of fase 2 when the arm reaches its maximum value. 
+bool            flag_v_arm                  = false;                        // 
+float           problem_velocity            = 0;                            // 
 
 //**************************** VARIABLES FOR CONTROL 4 *************************************
 float           reset_position              = 0;                            // The reset position of the arm.
@@ -158,10 +158,9 @@
 void SENSOR_LOOP()
 {
     Encoderread1 = wheel1.getPulses();
-            pos_arm = (Encoderread1/((64.0*131.0)/360.0));                  // Omrekenen naar graden van arm
+    previous_pos_arm = pos_arm;
+    pos_arm = (Encoderread1/((64.0*131.0)/360.0));                  // Omrekenen naar graden van arm
     pos_arm1 = pos_arm;
-    
-    previous_pos_arm = pos_arm;
     v_arm = (pos_arm - previous_pos_arm)/dt;
 
     Encoderread2 = wheel2.getPulses();
@@ -176,7 +175,6 @@
     if(flag_calibration == false) 
     {
         hoog_laag_filter();
-        sample_go = 0;
         sample_go = 1;
     }
 }
@@ -293,7 +291,7 @@
         }
         //************************* FASE SWITCH ******************************************
         //******************** Fase determination *************
-        if (fase_switch_wait == true) 
+        if (fase_switch_wait) 
         {
             j++;
             wait(0.01);                                              // Problemen met EMG metingen die te hoog zijn op het begin van script na calibratie. vandaar ff wachten en de sample loop een paar keer doorlopen.
@@ -338,7 +336,7 @@
                         motor2speed.write(relax);
                     }
 
-                    if (flag_left == true) 
+                    if (flag_left) 
                     {
                         if (y1> contract) 
                         {
@@ -351,7 +349,7 @@
                         }
                     }
 
-                    if (flag_right == true) 
+                    if (flag_right) 
                     {
                         if (y2 > contract) 
                         {
@@ -449,14 +447,17 @@
                         while(problem2) 
                         {
                             motor1direction.write(ccw);
-                            if(pos_arm < 170){
-                                if(v_arm > 200){
+                            if(pos_arm < 170)
+                            {
+                                if(v_arm > 200)
+                                {
                                     flag_v_arm = true;
-                                    }
                                 }
-                            if(flag_v_arm){
+                            }
+                            if(flag_v_arm)
+                            {
                                 v_arm_com = v_arm;
-                                }    
+                            }    
                             speed_control_arm = (0.4*((pos_arm2 - reset_arm)/500.0) + (0.05) - (v_arm_com/304)*0.3);
                             motor1speed.write(speed_control_arm);
 
@@ -544,7 +545,7 @@
                                 board = true;
                             }
                             
-                            if(board == true) 
+                            if(board) 
                             {
                                 pc.printf("fase switch  naar 1\n\n");
                                 board = false;