File content as of revision 0:113ea09a1ea9:

//#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 =.35;
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;
    
    int line_left_at;
    int line_right_at;
    bool line_left = false;
    bool line_right = false;
    int left_counter  =0;
    int right_counter =0;
    

    // 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==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
                    
                    for (uint16_t i=100; i>0; i--) {
                        line_left = (*(TFC_LineScanImage0+i) < 250) and (*(TFC_LineScanImage0+i-1) < 250) and (*(TFC_LineScanImage0+i-2) < 250);
                        if (line_left) {
                            line_left_at = i;
                            i=-1;
                            TFC_SetBatteryLED(1);
                        }
                    }
                    
                    for (uint16_t i=40; i<128; i++) {
                        line_right = (*(TFC_LineScanImage1+i) < 250) and (*(TFC_LineScanImage1+i+1) < 250) and (*(TFC_LineScanImage1+i+2) < 250);
                        if (line_right) {
                            line_right_at = i;
                            i=129;
                            TFC_SetBatteryLED(4);
                        }
                    }
                   
                    
                    // turn left
                    if (line_right) {
                        
                    }
                    
                    // turn right
                    if (line_left) {

                    }

                    // clearing values for next image processing round
                    line_left_at=63;
                    line_right_at=63;
                    line_left = false;
                    line_right = 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
    }//end while true
}//end main


/* shitcode
left at bool, int
right at bool, int

left turn counter
right turn counter

if bool
    inc counter by proportional to int (where line is)
    turn away proportional to counter
    multiply counter by .75 (turn less)
    
*/