File content as of revision 37:56dc8c2e8cf9:

//#include "mbed.h"
#include "TFC.h"
#include <iostream>
#include <stdio.h>
//#include "serialib.h"

const float AGGRESSIVE2 = .43;
const float AGGRESSIVE = .40;
const float MODERATE =.35;
const float CONSERVATIVE =.30;
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 current_left_motor_speed = 0;
    float current_right_motor_speed = 0;
    
    // gains on prop, int, der
    // subject to change, need to fine tune
    
    bool rear_motor_enable_flag = true;
    bool linescan_ping_pong = false;
    bool linescan_enable = true;
    
    bool whiteflag = false;
    bool blackflag = false;
    bool stopflag = false;
    
    int black_list_one = 0;
    int white_list = 0;
    int black_list_two = 0;

    int black_values_list[LINE_SCAN_LENGTH];
    int black_value_count = 0;
    int black_value_count_previous = 0;
    int black_center_value = 0;
    int sum_black = 0;
    int violence_level = 0;
    
    int center_now = 63;
    int center_past_1 = 63;
    int center_past_2 = 63;
    int center_past_3 = 63;
    int center_past_4 = 63;
    
    float left_counter  =0;
    float right_counter =0;
    bool turn_left=false;
    bool turn_right=false;

    float bullshit_offset = .08;
    
    int num_of_straight =0;
    int num_of_left =0;
    int num_of_right=0;
    
    int values_list_add =0;
    int value_count =0;
    int avg_value=0;
    //int black_threshhold=0;
    int black_threshhold =450;

    // major loop
    while(1) {

        // initial motor stuff
        if(rear_motor_enable_flag) {
            TFC_HBRIDGE_ENABLE;

            // checking behavior level
            violence_level =  int(TFC_GetDIP_Switch());

            
            if (violence_level==4) {
                current_left_motor_speed  = -(AGGRESSIVE2);
                current_right_motor_speed = AGGRESSIVE2;
            }
            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 for center line (single line)
                    for (uint16_t i=10; i<118; i++) {

                        int black_threshhold =600*(TFC_ReadPot(0));
                        if ((*(TFC_LineScanImage0+i) < black_threshhold)) {
                            black_values_list[black_value_count] = i;
                            black_value_count++;
                            //black_list_one++;
                        }
                        /*
                        else
                        {
                            black_list_one = 0;
                        }
                        
                        if(black_list_one > 5)
                        {
                            for(uint16_t j = i+black_list_one; j<118; j++)
                            {
                                if((*(TFC_LineScanImage0+j) > black_threshhold))
                                {
                                    white_list++;
                                    whiteflag = true;
                                }
                                else
                                {
                                    whiteflag = false;
                                }
                                if(!whiteflag)
                                {
                                    j = 200;
                                }
                                else if(whiteflag and white_list > 4)
                                {
                                    for(uint16_t k = j+white_list; k < 118; k++)
                                    {
                                        if((*(TFC_LineScanImage0+k) < black_threshhold))
                                        {
                                            black_list_two++;
                                            blackflag = true;
                                        }
                                        else
                                        {
                                            blackflag = false;
                                        }
                                        if(!blackflag)
                                        {
                                            k = 200;
                                        }
                                        else if(blackflag and black_list_two > 3)
                                        {
                                            stopflag = true;
                                        }
                                    }
                                }
                            }
                        }
                        */
                        
                    }

                    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;
                    */
                    
                    black_value_count_previous = black_value_count;
                    
                    //if (black_value_count>2)
                    center_now = sum_black / black_value_count;
                    
                    uint8_t num = 0;
                    
                    if(center_now > 10 && center_now < 19) // turn right little
                        num = 1;
                    else if(center_now >= 19 && center_now < 39)// moderate right
                        num = 3;
                    else if(center_now >= 39 && center_now < 62)// snap right
                        num = 2;
                    else if(center_now >= 62 && center_now < 65) // center do nothing
                        num = 15;
                    else if(center_now >= 65 && center_now < 89)// snap left
                        num = 4;
                    else if(center_now >=89 && center_now < 109) // moderate left
                        num = 12;
                    else if(center_now >= 109 && center_now < 118) // turn left little
                        num = 8;
                    
                    else
                        num = 0;
                    
                    if (black_value_count<2)
                        num = 0;
                        
                    //if (black_value_count>26 and ((*(TFC_LineScanImage0+64) > 450) or (*(TFC_LineScanImage0+63) > 450)or(*(TFC_LineScanImage0+62) > 450)))
                    //if (black_value_count + black_value_count_previous > 30 && (black_value_count > 15 or black_value_count_previous > 15) )
                    // if(black_value_count > 22)
                    //if (stopflag)
                    if(0)
                    {
                        while(1)
                            TFC_SetMotorPWM(0, 0);
                    }    
                    
                    TFC_SetBatteryLED(num);
                    
                    // best guess of center based on weighted average of history
                    black_center_value = center_now;
  
                    
                    // turn left
                    if (num==8 and right_counter <.35)
                    {
                     /*   
                        if (center_now <113)
                            left_counter=-0.4;
                        else
                        {
                            if (left_counter > -.15)
                                left_counter=-0.15; // less drastic for outside parts
                        }
                        turn_left=true;
                        turn_right=false;
                        */
                        
                        
                        
                            if (left_counter > -.15)
                                left_counter=-0.15; // less drastic for outside parts
                        
                        turn_left=true;
                        turn_right=false;
                        

                    }
                    else if(num == 12 and right_counter <.35)
                    {
                        if(left_counter > -.4)
                            left_counter=-0.4;
                        turn_left = true;
                        turn_right = false;
                    }
                
                    else if (num==4 and right_counter <.45)
                    {
                        left_counter=-0.6;
                        //left_counter=-0.55;
                        turn_left=true;
                        turn_right=false;
                    }
                    
                    // need to turn right
                    else if (num==1 and left_counter >-.35)
                    {
                        /*
                        if (center_now >15)
                            right_counter =.4;
                        else 
                        {
                            */
                            if (right_counter <.15)
                                right_counter =.15; // less drastic for outside parts
                        
                        
                        turn_left=false;
                        turn_right=true;
                   
                    }
                    
                    else if(num == 3 and left_counter >-.35)
                    {
                        if(right_counter < 0.4)
                            right_counter = 0.4;
                            
                        turn_left = false;
                        turn_right = true;
                    }
                
                      else if (num==2 and left_counter >-.45)
                    {
                        right_counter =.6;
                        //right_counter =.55;
                        turn_left=false;
                        turn_right=true;
                    }

                    //straight
                   else if (turn_right == false and turn_left == false)
                    {
                        //TFC_SetServo(0,(0.0+ bullshit_offset));
                        TFC_SetServo(0,bullshit_offset);
                        TFC_SetMotorPWM(current_left_motor_speed-(.0008*num_of_straight), current_right_motor_speed+(.0008*num_of_straight)); // --left is faster, ++right is faster
                        if (violence_level !=0)
                            num_of_straight++;
                    }
                   
                    else
                    {
                    }
                    
                    //dealwiththeshit
                    if(turn_left)
                    {
                        turn_right = false;
                        num_of_straight = 0; // no longer on a straight
                        
                        if (violence_level == 2 or violence_level == 1)
                            TFC_SetServo(0,left_counter+ bullshit_offset );
                        if (violence_level == 3 or violence_level == 4)
                            TFC_SetServo(0,(left_counter+ bullshit_offset)*.8 );
                        
                        //TFC_SetServo(0,left_counter);
                        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 is slower, ++right is faster
                        
                            
                            
                        }
                        
                    if(turn_right)
                    {
                        turn_left =false;
                        num_of_straight = 0; // no longer on a straight
                        
                        //TFC_SetServo(0,right_counter- bullshit_offset );
                        //TFC_SetServo(0,right_counter);
                        
                        if (violence_level == 2 or violence_level == 1)
                            TFC_SetServo(0,(right_counter+ bullshit_offset));
                        if (violence_level == 3 or violence_level == 4)
                            TFC_SetServo(0,(right_counter+ bullshit_offset)*.8 );            
                        
                        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)); // --left is faster, --right is slower       
                        }   

                    // clearing values for next image processing round
                    black_value_count = 0;
                    sum_black = 0;
                    values_list_add =0;
                    value_count =0;
                    avg_value=0;
                    black_threshhold=0;
                    
                    black_list_one = 0;
                    white_list = 0;
                    black_list_two = 0;
                    whiteflag = false;
                    blackflag = false;
                    stopflag = false;

                    // 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