Diff: main.cpp

Revision:

4:f9f75c913d7d

Parent:

3:f9a1df2271d2

Child:

5:0597358d0016

--- a/main.cpp	Fri Oct 23 08:24:34 2015 +0000
+++ b/main.cpp	Fri Oct 23 09:47:23 2015 +0000
@@ -48,8 +48,10 @@
 const int       relax                   = 0;
 int             games_played            = -1;                                    // -1 omdat het spel daar eerst loopt voor het spelen om alles na te checken
 int             games_played1           = 0;
+float           dt                      = 0.01;
 bool            flag_calibration        = true;
 
+
 //********* VARIABLES FOR MOTOR CONTROL ********
 const float     cw                      = 1;                          // The number if: motor1 moves clock wise motor2 moves counterclockwise
 const float     ccw                     = 0;                          // The number if: motor1 moves counterclock wise motor2 moves clockwise
@@ -112,12 +114,17 @@
 const float     maximum_throw_angle     = 110;
 float           pos_arm                 = 0;
 int             pos_arm1                = 0;
+float           previous_pos_arm        = 0;
+float           v_arm                   = 0;
+float           v_arm_com               = 0;
+float           speed_control_arm       = 0.000;
 float           Encoderread1            = 0;
 int             switch_rondjes          = 2;
 int             rondjes                 = 0;
 float           pos_armrondjes_max      = 3;
 bool            problem1                = false;
 bool            problem2                = false;
+bool            flag_v_arm              = false;
 float           problem_velocity        = 0;
 
 //********* VARIABLES FOR CONTROL 4 ************
@@ -162,6 +169,9 @@
     Encoderread1 = wheel1.getPulses();
     pos_arm = (Encoderread1/((64.0*131.0)/360.0));   // Omrekenen naar graden van arm
     pos_arm1 = pos_arm;
+    
+    v_arm = (pos_arm - previous_pos_arm)/dt;
+    previous_pos_arm = pos_arm;
 
     Encoderread2 = wheel2.getPulses();
     pos_board = (Encoderread2/((64.0*131.0)/360.0));   // Omrekenen naar graden van board
@@ -411,7 +421,8 @@
 
                     if (y2 > minimum_contract & y2 < medium_contract) 
                     {
-                        motor1speed.write((y2 - minimum_contract)/(medium_contract - minimum_contract));
+                        speed_control_arm = ((v_arm/304) + 0.15*(1 -(v_arm/304)));
+                        motor1speed.write(speed_control_arm);
                     }
                     if (y2 > medium_contract)                // Hoger dan drempelwaarde = Actief
                     {    
@@ -458,15 +469,20 @@
                         while(problem2) 
                         {
                             motor1direction.write(ccw);
-                            motor1speed.write(0.02);
+                            if(pos_arm < 170){
+                                if(v_arm > 200){
+                                    flag_v_arm = true;
+                                    }
+                                }
+                            if(flag_v_arm){
+                                v_arm_com = v_arm;
+                                }    
+                            speed_control_arm = (0.4*((pos_arm1 - reset_arm)/500.0) + (0.05) - (v_arm_com/304)*0.3);
+                            motor1speed.write(speed_control_arm);
 
-                            if (flag_s) 
-                            {
-                                Encoderread1 = wheel1.getPulses();
-                                pos_arm = (Encoderread1/((64.0*131.0)/360.0));   // Omrekenen naar graden van arm
-                            }
                             if (pos_arm < 10) 
                             {
+                                flag_v_arm = false;
                                 problem2 = false;
                                 motor1speed.write(0);
                                 rondjes = 0;