File content as of revision 17:c643b6b4a96f:

//#include "mbed.h"
#include "TFC.h"

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;
    
    float dt = 0.00001;
    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[128];
    int black_value_count = 0;
    int black_center_value = 0;
    int sum_black = 0;
    int violence_level = 0;
    
    int set_point = 64;
    int previous error = 0;
    int error = 0;

    
    //uint16_t  MyImage0Buffer[2][128];
    //uint16_t  MyImage1Buffer[2][128];

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

            //current_left_motor_speed    = (TFC_ReadPot(0));
            //current_right_motor_speed   = (TFC_ReadPot(1));

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

            if (violence_level==2) {
                current_left_motor_speed  = -.48;
                current_right_motor_speed = .48;
            }
            else if (violence_level==1) {
                current_left_motor_speed  = -.35;
                current_right_motor_speed = .35;
            }
            else if (violence_level==0) {
                current_left_motor_speed  = 0;
                current_right_motor_speed = 0;
            }
            else {
                current_left_motor_speed  = 0;
                current_right_motor_speed = 0;
            }
            

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

            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;
                    if (*(TFC_LineScanImage0+64) > 800)
                        shitnum = 15;
                    else if((*(TFC_LineScanImage0+64) > 450))
                        shitnum = 7;
                    else if((*(TFC_LineScanImage0+64) > 400))
                        shitnum = 3;
                    else
                        shitnum = 1;
                    TFC_SetBatteryLED(shitnum);


                    // checking for center line (single line)
                    for (uint16_t i=0; i<128; i++) {
                        if ((*(TFC_LineScanImage0+i) < 300)) {
                            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];
                    }

                    // value of center of black (single line)
                    black_center_value= sum_black / black_value_count;
                    
                    /* ******* PID ALGORITHM *******
                    
                    error = set_point - black_center_value;
                    integral = integral + error*dt;
                    derivative = (error - previous_error)/dt;
                    output = kp*error + ki*integral + kd*derivative;
                    previous error = error;
                    
                    GOTTA DO SOME TESTS/SIMULATIONS TO CALIBRATE SERVO ADJUSTMENTS
                    
                    
                       *****************************
                    */

                    // need to turn left
                    if (black_center_value < 64) {
                        
                        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);


                        //current_left_motor_speed  = current_left_motor_speed  + float(64-black_center_value)*.0025;
                        //current_right_motor_speed = current_right_motor_speed + float(64-black_center_value)*.0025;
                        if (violence_level !=0){
                        current_left_motor_speed  = current_left_motor_speed  + float(float(64-black_center_value)*.045);// kinda reverse this...
                        current_right_motor_speed = current_right_motor_speed + float(float(64-black_center_value)*.045);// push more forwards
                        }
                        
                        // 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;
                        
                        TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);

                    }

                    // need to turn right
                    if (black_center_value > 64) {

                        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);

                        //current_left_motor_speed  = current_left_motor_speed  - float(black_center_value-64)*.0025;
                        //current_right_motor_speed = current_right_motor_speed - float(black_center_value-64)*.0025;
                        if (violence_level !=0){
                        current_left_motor_speed  = current_left_motor_speed  - float(float(black_center_value-64)*.045);// push more forwards
                        current_right_motor_speed = current_right_motor_speed - float(float(black_center_value-64)*.045);// kinda reverse this...
                        }

                        // 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;
                            
                        TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);

                    }

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