Diff: main.cpp

Revision:

21:df5a530208eb

Parent:

20:c728b8ffad97

Child:

22:f097de115024

--- a/main.cpp	Fri Feb 27 17:10:15 2015 +0000
+++ b/main.cpp	Mon Mar 02 17:59:55 2015 +0000
@@ -90,6 +90,10 @@
                 current_left_motor_speed  = -.35;
                 current_right_motor_speed = .35;
             }
+             else if (violence_level==3) {
+                current_left_motor_speed  = -.55;
+                current_right_motor_speed = .55;
+            }
             else if (violence_level==0) {
                 current_left_motor_speed  = 0;
                 current_right_motor_speed = 0;
@@ -101,14 +105,14 @@
             
 
             // protection block
-            if(current_left_motor_speed >= 0.5)
-                current_left_motor_speed= 0.5;
-            if(current_right_motor_speed >= 0.5)
-                current_right_motor_speed= 0.5;
-            if(current_left_motor_speed <= -0.5)
-                current_left_motor_speed= -0.5;
-            if(current_right_motor_speed <= -0.5)
-                current_right_motor_speed= -0.5;
+            if(current_left_motor_speed >= 0.7)
+                current_left_motor_speed= 0.7;
+            if(current_right_motor_speed >= 0.7)
+                current_right_motor_speed= 0.7;
+            if(current_left_motor_speed <= -0.7)
+                current_left_motor_speed= -0.7;
+            if(current_right_motor_speed <= -0.7)
+                current_right_motor_speed= -0.7;
 
             TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
         }// end motor enabled
@@ -138,7 +142,7 @@
 
                     // checking for center line (single line)
                     for (uint16_t i=0; i<128; i++) {
-                        if ((*(TFC_LineScanImage0+i) < 300)) {
+                        if ((*(TFC_LineScanImage0+i) < 260)) {
                             black_values_list[black_value_count] = i;
                             black_value_count++;
                         }
@@ -159,7 +163,7 @@
                     center_now = sum_black / black_value_count;
                     
                     // best guess of center based on weighted average of history
-                    black_center_value = (5*center_now + 10*center_past_1 + 15*center_past_2 +30*center_past_3 +40*center_past_4)/100;
+                    black_center_value = (5*center_now + 5*center_past_1 + 10*center_past_2 +10*center_past_3 +70*center_past_4)/100;
                     
                     /* ******* PID ALGORITHM *******
                     
@@ -175,6 +179,15 @@
                        *****************************
                     */
 
+                    /*if (black_center_value == 64) {
+                        
+                        TFC_SetServo(0, 0.0);
+                        if (violence_level !=0){
+                            current_left_motor_speed  = current_left_motor_speed  + violence_level*.045;// push more forwards
+                            current_right_motor_speed = current_right_motor_speed + violence_level*.045;// push more forwards
+                            }
+                        }*/
+
                     // need to turn left
                     if (black_center_value < 64) {
                         
@@ -187,11 +200,11 @@
                         //current_left_motor_speed  = current_left_motor_speed  + float(64-black_center_value)*.0025;
                         //current_right_motor_speed = current_right_motor_speed + float(64-black_center_value)*.0025;
                         if (violence_level !=0){
-                        current_left_motor_speed  = current_left_motor_speed  + float(float(64-black_center_value)*.025);// kinda reverse this...
-                        current_right_motor_speed = current_right_motor_speed + float(float(64-black_center_value)*.045);// push more forwards
+                        current_left_motor_speed  = current_left_motor_speed  + float(float(64-black_center_value)*.035);// kinda reverse this...
+                        current_right_motor_speed = current_right_motor_speed + float(float(64-black_center_value)*.025);// push more forwards
                         }
                         
-                        // protection block
+                        /*// protection block
                         if(current_left_motor_speed >= 0.5)
                             current_left_motor_speed= 0.5;
                         if(current_right_motor_speed >= 0.5)
@@ -199,7 +212,17 @@
                         if(current_left_motor_speed <= -0.5)
                             current_left_motor_speed= -0.5;
                         if(current_right_motor_speed <= -0.5)
-                            current_right_motor_speed= -0.5;
+                            current_right_motor_speed= -0.5;*/
+                            
+                        // protection block
+                        if(current_left_motor_speed >= 0.7)
+                            current_left_motor_speed= 0.7;
+                        if(current_right_motor_speed >= 0.7)
+                            current_right_motor_speed= 0.7;
+                        if(current_left_motor_speed <= -0.7)
+                            current_left_motor_speed= -0.7;
+                        if(current_right_motor_speed <= -0.7)
+                            current_right_motor_speed= -0.7;
                         
                         TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
 
@@ -216,11 +239,11 @@
                         //current_left_motor_speed  = current_left_motor_speed  - float(black_center_value-64)*.0025;
                         //current_right_motor_speed = current_right_motor_speed - float(black_center_value-64)*.0025;
                         if (violence_level !=0){
-                        current_left_motor_speed  = current_left_motor_speed  - float(float(black_center_value-64)*.045);// push more forwards
-                        current_right_motor_speed = current_right_motor_speed - float(float(black_center_value-64)*.025);// kinda reverse this...
+                        current_left_motor_speed  = current_left_motor_speed  - float(float(black_center_value-64)*.025);// push more forwards
+                        current_right_motor_speed = current_right_motor_speed - float(float(black_center_value-64)*.035);// kinda reverse this...
                         }
 
-                        // protection block
+                        /*// protection block
                         if(current_left_motor_speed >= 0.5)
                             current_left_motor_speed= 0.5;
                         if(current_right_motor_speed >= 0.5)
@@ -228,7 +251,17 @@
                         if(current_left_motor_speed <= -0.5)
                             current_left_motor_speed= -0.5;
                         if(current_right_motor_speed <= -0.5)
-                            current_right_motor_speed= -0.5;
+                            current_right_motor_speed= -0.5;*/
+                            
+                        // protection block
+                        if(current_left_motor_speed >= 0.7)
+                            current_left_motor_speed= 0.7;
+                        if(current_right_motor_speed >= 0.7)
+                            current_right_motor_speed= 0.7;
+                        if(current_left_motor_speed <= -0.7)
+                            current_left_motor_speed= -0.7;
+                        if(current_right_motor_speed <= -0.7)
+                            current_right_motor_speed= -0.7;
                             
                         TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);