Diff: asof3_25_2015_2200hours.txt

Revision:

13:8a667757beb3

Parent:

12:915f22e7d7d9

Child:

14:1a408e13679d

--- a/asof3_25_2015_2200hours.txt	Thu Mar 26 19:41:04 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,352 +0,0 @@
-//#include "mbed.h"
-#include "TFC.h"
-#include <iostream>
-#include <stdio.h>
-//#include "serialib.h"
-
-const float AGGRESSIVE = .55;
-const float MODERATE =.48;
-const float CONSERVATIVE =.39;
-const float STOP =0;
-const float PROTECTION_THRESHOLD_UPPER =.7;
-const float PROTECTION_THRESHOLD_LOWER =-.7;
-const float TURN_FORWARD_ACCEL =0.045;
-const float TURN_BACKWARD_ACCEL =0.025;
-const float SERVO_CAN_MOVE_IN_ONE_FRAME =0.1;
-const float SERVO_MAX =.5;
-
-const int BLACK_THRESHOLD =63;
-const int LINE_SCAN_LENGTH =128;
-
-
-DigitalOut myled(LED1);
-
-int main()
-{
-    //run this before anything
-    TFC_Init();
-
-    //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;
-    bool linescan_enable = true;
-
-    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 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;
-    bool turn_left=false;
-    bool turn_right=false;
-
-    float bullshit_offset = .074;
-
-    // major loop
-    while(1) {
-
-        // manual servo control, unused
-        if (TFC_ReadPushButton(0) != 0 ) {
-            current_servo_position = current_servo_position-.005;
-            if(current_servo_position <= -0.4)
-                current_servo_position = -0.4;
-            TFC_SetServo(0, current_servo_position);
-        }// end check button0
-
-        else {}
-        
-
-        // manual servo control, unused
-        if (TFC_ReadPushButton(1) != 0 ) {
-            current_servo_position = current_servo_position+.005;
-            if(current_servo_position >= 0.4)
-                current_servo_position = 0.4;
-            TFC_SetServo(0, current_servo_position);
-        }// end check button1
-
-        else {}
-
-        // initial motor stuff
-        if(rear_motor_enable_flag) {
-            TFC_HBRIDGE_ENABLE;
-
-
-
-            // checking behavior level
-            violence_level =  int(TFC_GetDIP_Switch());
-
-            if (violence_level==3) {
-                current_left_motor_speed  = -(AGGRESSIVE);
-                current_right_motor_speed = AGGRESSIVE;
-            }
-            else if (violence_level==2) {
-                current_left_motor_speed  = -(MODERATE);
-                current_right_motor_speed = (MODERATE);
-            }
-            else if (violence_level==1) {
-                current_left_motor_speed  = -(CONSERVATIVE);
-                current_right_motor_speed = CONSERVATIVE;
-            }
-            else if (violence_level==0) {
-                current_left_motor_speed  = STOP;
-                current_right_motor_speed = STOP;
-            }
-            else {
-                current_left_motor_speed  = STOP;
-                current_right_motor_speed = STOP;
-            }
-            
-
-            // protection block
-            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
-        else {
-            TFC_HBRIDGE_DISABLE;
-        }// end motor disabled
-
-        // camera stuff
-        if (linescan_enable) {
-            if (TFC_LineScanImageReady !=0) {
-
-                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=15; i<113; i++) {
-                        if ((*(TFC_LineScanImage0+i) < 450)) {
-                            black_values_list[black_value_count] = i;
-                            black_value_count++;
-                        }
-                    }
-
-                    for(int i=0; i<black_value_count; i++) {
-                        sum_black += black_values_list[i];
-                    }
-
-                    //update history
-                    center_past_4= center_past_3;
-                    center_past_3= center_past_2;
-                    center_past_2= center_past_1;
-                    center_past_1= center_now;
-                    
-                    
-                    //if (black_value_count>2)
-                    center_now = sum_black / black_value_count;
-                    
-                    uint8_t num = 0;
-                    
-                    if(center_now > 15 && center_now < 27)
-                        num = 1;
-                    else if(center_now >= 27 && center_now < 54)
-                        num = 2;
-                    else if(center_now > 60 && center_now < 70)
-                        num = 15;
-                    else if(center_now >= 54 && center_now < 81)
-                        num = 4;
-                    else if(center_now >= 81 && center_now < 113)
-                        num = 8;
-                    
-                    else
-                        num = 0;
-                    
-                    if (black_value_count<2)
-                        num = 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 )
-                    {
-                        //left_counter += (128-black_center_value);
-                        //left_counter+=20;
-                        
-                        /*
-                        if(left_counter<-0.38)
-                            left_counter=-0.5;
-                        else
-                            left_counter= -0.4;
-                            */
-                            
-                        if (left_counter >-.5)
-                            left_counter -=.1;    
-                            
-                        turn_left=true;
-                        turn_right=false;
-
-                    }
-                    if (num==4 and right_counter <.2)
-                    {
-                        //left_counter += (128-black_center_value);
-                        //left_counter+=20;
-                        left_counter=-0.56;
-                        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)
-                    {
-                        //right_counter += black_center_value;
-                        
-                        //right_counter +=20;
-                        /*
-                        if (right_counter >.38)
-                            right_counter =.5;
-                        else
-                            right_counter =.4;
-                          */
-                        
-                        if (right_counter <.5)
-                            right_counter +=.1;
-                        
-                        turn_left=false;
-                        turn_right=true;
-                   
-                    }
-                      else if (num==2 and left_counter >-.2)
-                    {
-                        //right_counter += black_center_value;
-                        //right_counter +=20;
-                        right_counter =.56;
-                        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)
-                    {
-                        TFC_SetServo(0,(0.0+ bullshit_offset));
-                    }
-                   
-                    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);
-                        left_counter += .01;
-                        if (left_counter > (0+ bullshit_offset))
-                            turn_left = false;
-                            
-                        TFC_SetMotorPWM(current_left_motor_speed+(.1*left_counter), current_right_motor_speed+(.3*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);
-                        right_counter -= .01;
-                        if (right_counter < (0+ bullshit_offset))
-                            turn_right = false;
-                        
-                        TFC_SetMotorPWM(current_left_motor_speed-(.3*right_counter), current_right_motor_speed-(.1*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
-
-                    linescan_ping_pong = false;
-                } // end checking channel 0
-                
-                else { //checking channel 1
-                    linescan_ping_pong = true;
-                }
-
-                TFC_LineScanImageReady = 0;  // since we used it, we reset the flag
-            }// end imageready
-        }// end linescan stuff
-    }
-}
-
-// shit code down here