Diff: main.cpp

Revision:

29:4bf08c74f792

Parent:

28:add6c7e2f0d3

Child:

30:8e0ad64703d0

--- a/main.cpp	Sat Mar 28 21:54:12 2015 +0000
+++ b/main.cpp	Sat Mar 28 22:04:46 2015 +0000
@@ -28,21 +28,11 @@
 
     //variables
     float current_servo_position = 0;
-    float previous_servo_position = 0;
     float current_left_motor_speed = 0;
     float current_right_motor_speed = 0;
     
-    float proportional = 0;
-    float last_proportional = 0;
-    float integral = 0;
-    float derivative = 0;
-    float output = 0;
-    
     // gains on prop, int, der
     // subject to change, need to fine tune
-    float kp = 1.8960;
-    float ki = 0.6170;
-    float kd = 1.5590;
     
     bool rear_motor_enable_flag = true;
     bool linescan_ping_pong = false;
@@ -54,25 +44,11 @@
     int sum_black = 0;
     int violence_level = 0;
     
-    int accelList[3];
-    int lastAccessed = 0;
-
-    int centers_List[50];
-    
     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 position = 0;
-    int set_point = 63;
-    int previous_error = 0;
-    int error = 0;
-
-    for(int i = 0; i < 50; i++)
-        centers_List[i] = 63;
     
     float left_counter  =0;
     float right_counter =0;
@@ -109,8 +85,6 @@
         if(rear_motor_enable_flag) {
             TFC_HBRIDGE_ENABLE;
 
-
-
             // checking behavior level
             violence_level =  int(TFC_GetDIP_Switch());
 
@@ -159,10 +133,6 @@
                 if (linescan_ping_pong) {
                     //checking channel 0
                     
-                    //checking center pixel, displays aprox value on leds
-                    uint8_t shitnum = 1;
-                    
-  
                     // checking for center line (single line)
                     for (uint16_t i=10; i<118; i++) {
                         if ((*(TFC_LineScanImage0+i) < 450)) {
@@ -204,23 +174,19 @@
                     if (black_value_count<2)
                         num = 0;
                         
-                    if (black_value_count>30)
+                    if (black_value_count>27)
                     {
                         while(1)
                             TFC_SetMotorPWM(0, 0);
                     }    
-                        
                     
                     TFC_SetBatteryLED(num);
                     
                     // 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 = (15*center_now + 15*center_past_1 + 15*center_past_2 +25*center_past_3 +30*center_past_4)/100;
                     black_center_value = center_now;
   
                     
                     // turn left
-                    //if (black_center_value > BLACK_THRESHOLD+30) {
                     if (num==8 and right_counter <.2 )
                     {
                         
@@ -237,16 +203,12 @@
                     }
                     if (num==4 and right_counter <.2)
                     {
-                        //left_counter += (128-black_center_value);
-                        //left_counter+=20;
                         left_counter=-0.6;
                         turn_left=true;
                         turn_right=false;
-
                     }
                     
                     // need to turn right
-                    //else if (black_center_value < BLACK_THRESHOLD-30) {
                     else if (num==1 and left_counter >-.2)
                     {
                         if (center_now >15)
@@ -263,24 +225,11 @@
                     }
                       else if (num==2 and left_counter >-.2)
                     {
-                        //right_counter += black_center_value;
-                        //right_counter +=20;
                         right_counter =.6;
                         turn_left=false;
                         turn_right=true;
-                   
                     }
-                    //else if (black_value_count < 2)
-                   // {
-                   //     turn_right=false;
-                   ////     turn_left=false;
-                   //     TFC_SetServo(0,0.0);
-                   // }
-                   
-                   //else if (num==0)
-                    //{
-                     //   TFC_SetServo(0,0.0);
-                    //}
+
                    
                    
                    else if (turn_right == false and turn_left == false)
@@ -290,45 +239,34 @@
                    
                     else
                     {
-                        //turn_right=false;
-                        //turn_left=false;
-                        //TFC_SetServo(0,0.0);
                     }
                     
                     //dealwiththeshit
                     if(turn_left)
                     {
                         turn_right = false;
-                        //TFC_SetServo(0, ((left_counter)*(-.05)));
+                        
                         TFC_SetServo(0,left_counter+ bullshit_offset );
                         left_counter += .01;
                         if (left_counter > (0+ bullshit_offset))
                             turn_left = false;
                             
                         TFC_SetMotorPWM(current_left_motor_speed+(.2*left_counter), current_right_motor_speed+(.2*left_counter));  
-                        //left_counter -= 1;
-                        //if (left_counter < 10)
-                        //    turn_left =false; 
                         }
                         
                     if(turn_right)
                     {
                         turn_left =false;
-                        //TFC_SetServo(0, ((right_counter)*(.05)));
                         TFC_SetServo(0,right_counter- bullshit_offset );
                         right_counter -= .01;
                         if (right_counter < (0+ bullshit_offset))
                             turn_right = false;
                         
                         TFC_SetMotorPWM(current_left_motor_speed-(.2*right_counter), current_right_motor_speed-(.2*right_counter));        
-                        //right_counter -= 1;
-                       // if (right_counter < 10)
-                       //     turn_right =false; 
                         }   
 
                     // clearing values for next image processing round
                     black_value_count = 0;
-                    //black_center_value = 0;
                     sum_black = 0;
 
                     // end image processing