File content as of revision 14:f43b386b8b5d:

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


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


    // major loop
    while(1) {

        if (TFC_ReadPushButton(0) != 0 ) {
            /*(TFC_BAT_LED0_ON;
            wait(0.004);
            TFC_BAT_LED0_OFF;
            wait(0.004);*/

            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 {
            //TFC_BAT_LED0_ON;
        }

        if (TFC_ReadPushButton(1) != 0 ) {
            /*TFC_BAT_LED1_ON;
            wait(0.004);
            TFC_BAT_LED1_OFF;
            wait(0.004);*/

            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 {
            //TFC_BAT_LED1_ON;
        }


        if(rear_motor_enable_flag) {
            TFC_HBRIDGE_ENABLE;
            //current_left_motor_speed    = current_left_motor_speed +  .3*(TFC_ReadPot(0));
            //current_right_motor_speed   = current_right_motor_speed +  .3*(TFC_ReadPot(1));

            //current_left_motor_speed    = .3*(TFC_ReadPot(0));
            //current_right_motor_speed   = .3*(TFC_ReadPot(1));

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

            violence_level =  int(TFC_GetDIP_Switch());

            if (violence_level==1) 
            {
                current_left_motor_speed  = .2;
                current_right_motor_speed = .2;
            }

            if(current_left_motor_speed >= 0.4)
                current_left_motor_speed= 0.4;
            if(current_right_motor_speed >= 0.4)
                current_right_motor_speed= 0.4;
            if(current_left_motor_speed <= -0.4)
                current_left_motor_speed= -0.4;
            if(current_right_motor_speed <= -0.4)
                current_right_motor_speed= -0.4;

            TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
        }// end motor enabled
        else {
            TFC_HBRIDGE_DISABLE;
        }// end motor disabled

        if (linescan_enable) {
            if (TFC_LineScanImageReady !=0) {

                if (linescan_ping_pong) { //checking channel 0
                    //...
                    //uint8_t shitnum = uint8_t(*TFC_LineScanImage0>>3);
                    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?
                    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];
                    }

                    black_center_value= sum_black / black_value_count;

                    if (black_center_value < 64) {


                        //current_servo_position= float(-0.2);
                        /*if(black_center_value < 40)
                            current_servo_position = current_servo_position-.0004;
                        else
                            current_servo_position = current_servo_position-.0001;*/

                        //current_servo_position= float(.009375*black_center_value-(.6));
                        //current_servo_position= float(.00625*black_center_value-(.4));
                        current_servo_position= float(.015625*black_center_value-(1));
                        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;
                        
                        TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);

                    }

                    if (black_center_value > 64) {
                        //current_servo_position = current_servo_position+.0001;
                        //current_servo_position= float();
                        //current_servo_position= float(0.2);
                        /*if(black_center_value > 88)
                            current_servo_position = current_servo_position+.0004;
                        else
                            current_servo_position = current_servo_position+.0001;*/

                        //current_servo_position= float(.009375*black_center_value-(.6));
                        //current_servo_position= float(.00625*black_center_value-(.4));
                        current_servo_position= float(.015625*black_center_value-(1));
                        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;
                        
                        TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
                        
                    }

                    black_value_count = 0;
                    black_center_value = 0;
                    sum_black = 0;

                    // end checking for center line


                    linescan_ping_pong = false;
                } // end checking channel 0
                else {                  //checking channel 1
                    //...
                    //TFC_SetBatteryLED(*TFC_LineScanImage1+4);
                    linescan_ping_pong = true;
                }


                TFC_LineScanImageReady ==0;  // since we used it, we reset the flag
            }// end imageready
        }// end linescan stuff
    }
}



// shit code

//this block is the worst image processing algorithm ever
/*
black_values_list[];
black_value_count = 0;
black_center_value = 0;
sum_black = 0;

for (int i=0; i<128; i++)
{
    if (camera_data[i]< 100)
    {
        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];
}

black_center_value= sum_black / black_value_count;

if (black_center_value > 64)
    servo_left();

if (black_center_value < 64)
    servo_right();
*/