Diff: main.cpp

Revision:

33:e14ef9206a63

Parent:

29:91bda3a63f17

Parent:

31:0c2b49175036

Child:

34:fac57f153973

--- a/main.cpp	Tue Mar 03 19:53:12 2015 +0000
+++ b/main.cpp	Tue Mar 03 20:11:12 2015 +0000
@@ -6,6 +6,9 @@
 #define CONSERVATIVE .35
 #define STOP 0
 #define BLACK_THRESHOLD 64
+#define LINE_SCAN_LENGTH 128
+#define PROTECTION_THRESHOLD_UPPER .7
+#define PROTECTION_THRESHOLD_LOWER -.7
 
 DigitalOut myled(LED1);
 
@@ -34,7 +37,7 @@
     bool linescan_ping_pong = false;
     bool linescan_enable = true;
 
-    int black_values_list[128];
+    int black_values_list[LINE_SCAN_LENGTH];
     int black_value_count = 0;
     int black_center_value = 0;
     int sum_black = 0;
@@ -119,14 +122,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 >= PROTECTION_THRESHOLD_UPPER)
+                current_left_motor_speed= PROTECTION_THRESHOLD_UPPER;
+            if(current_right_motor_speed >= PROTECTION_THRESHOLD_UPPER)
+                current_right_motor_speed = PROTECTION_THRESHOLD_UPPER;
+            if(current_left_motor_speed <= PROTECTION_THRESHOLD_LOWER)
+                current_left_motor_speed = PROTECTION_THRESHOLD_LOWER;
+            if(current_right_motor_speed <= PROTECTION_THRESHOLD_LOWER)
+                current_right_motor_speed = PROTECTION_THRESHOLD_LOWER;
 
             TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
         }// end motor enabled
@@ -143,6 +146,7 @@
                     
                     //checking center pixel, displays aprox value on leds
                     uint8_t shitnum = 1;
+                    
                     if (*(TFC_LineScanImage0+64) > 800)
                         shitnum = 15;
                     else if((*(TFC_LineScanImage0+64) > 450))
@@ -205,8 +209,6 @@
                     
                        *****************************
                     */
-
-                    // need to turn left
                     
                     if (black_center_value == BLACK_THRESHOLD)
                     {
@@ -236,9 +238,9 @@
                         }
                     }
                     
-                    if (black_center_value < 64) {
+                    if (black_center_value < BLACK_THRESHOLD) {
                         
-                        current_servo_position= float(.01875*black_center_value-(1.2));
+                        current_servo_position = float(.01875*black_center_value-(1.2));
                         if(current_servo_position <= -0.4)
                             current_servo_position = -0.4;
                         TFC_SetServo(0, current_servo_position);
@@ -332,9 +334,8 @@
                         TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
 
                     }
-
                     // need to turn right
-                    if (black_center_value > BLACK_THRESHOLD) {
+                    else if (black_center_value > BLACK_THRESHOLD) {
 
                         current_servo_position= float(.01875*black_center_value-(1.2));
                         if( current_servo_position >= +0.4)