Diff: main.cpp

Revision:

31:0c2b49175036

Parent:

25:1d0f586aaf0c

Child:

33:e14ef9206a63

--- a/main.cpp	Tue Mar 03 18:16:34 2015 +0000
+++ b/main.cpp	Tue Mar 03 18:49:16 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,20 +37,20 @@
     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;
     int violence_level = 0;
     
-    int center_now = 64;
-    int center_past_1 = 64;
-    int center_past_2 = 64;
-    int center_past_3 = 64;
-    int center_past_4 = 64;
+    int center_now = 63;
+    int center_past_1 = 63;
+    int center_past_2 = 63;
+    int center_past_3 = 63;
+    int center_past_4 = 63;
     //int best_guess_center = 64;
     
-    int set_point = 64;
+    int set_point = 63;
     int previous_error = 0;
     int error = 0;
 
@@ -111,14 +114,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
@@ -135,6 +138,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))
@@ -186,10 +190,14 @@
                        *****************************
                     */
 
+                    if (black_center_value == BLACK_THRESHOLD) {
+                        current_servo_position = 0;
+                        TFC_SetServo(0, current_servo_position);
+                    }
                     // need to turn left
-                    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);
@@ -215,9 +223,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)