File content as of revision 43:9f541718be01:

#include "TFC.h"
#include <iostream>
#include <stdio.h>

const float proportional = 0;
const float last_proportional = 0;
const float integral = 0;
const float derivative = 0;
const float output = 0;

// these are definitely not right
const float kp = 0.05;
const float ki = 0.02;
const float kd = 0.02;

const int set_point = 63;
const int position = 0;
const int error = 0;

const float AGGRESSIVE = .60;
const float MODERATE =.45;
const float CONSERVATIVE =.30;
const float STOP =0;

const bool linescan_enable = true;
const bool image = true;

const int black_values_list[LINE_SCAN_LENGTH];
const int black_value_count = 0;
const int black_center_value = 0;
const int sum_black = 0;
const int violence_level = 0;

const float current_left_motor_speed = 0;
const float current_right_motor_speed = 0;

const float current_right_max = 0;
const float current_left_max = 0;


int main()
{
    TFC_Init();
    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==3) {
            current_left_motor_speed  = -(AGGRESSIVE);
            current_right_motor_speed = AGGRESSIVE;
            current_right_max = AGGRESSIVE;
            current_left_max = -AGGRESSIVE;
            
        }
        else if (violence_level==2) {
            current_left_motor_speed  = -(MODERATE);
            current_right_motor_speed = (MODERATE);
            current_right_max = MODERATE;
            current_left_max = -MODERATE;
            Kp = Kp * 0.75;
            Ki = Ki * 0.75;
            Kd = Kd * 0.75;
        }
        else if (violence_level==1) {
            current_left_motor_speed  = -(CONSERVATIVE);
            current_right_motor_speed = CONSERVATIVE;
            current_right_max = CONSERVATIVE;
            current_left_max = CONSERVATIVE;
            Kp = Kp * 0.5;
            Ki = Ki * 0.5;
            Kd = Kd * 0.5;
        }
        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
    
}
        
    void loop()
    {
        if (linescan_enable) 
        {
            if (TFC_LineScanImageReady !=0) 
            {
                // first image
                if(CurrentLineScanChannel)
                {
                    position = getSensorValues(true); // get position
                }
                // second image
                else
                {
                    position = getSensorValues(false);
                }
                error = pid_calc(position); // send position to PID algorithm to get error
                calcMotors(error); // send error to calculate both motors
            }
        }
        else
        {
            TFC_SetMotorPWM(STOP, STOP);
        }
    }
    
    int getSensorValues(bool image)
    {        
        if(image)
        {
            for (uint16_t i=0; i<128; i++) 
            {
                if ((*(TFC_LineScanImage0+i) < 250)) 
                {
                    black_values_list[black_value_count] = i;
                    black_value_count++;
                }
            }
        }
        else
        {
            for (uint16_t i=0; i<128; i++)
            {
                if ((*(TFC_LineScanImage1+i) < 250))
                {
                    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 = int(sum_black / black_value_count);
        return black_center_value;
        
    }

    int pid_calc(int pos)
    {
        position = pos;
        proportional = position - set_point;
        integral = integral + proportional;
        derivative= proportional - last_proportional;
        last_proportional = proportional;
        error = proportional*Kp + integral*Ki + derivative*Kd;
        return error;
    }
    
    void calcMotors(float error)
    {
        TFC_SetServo(0, error);
        
        if(error < 0)
        {
            current_left_motor_speed = current_left_motor_speed + error; // reverse this
            
            // make sure not going too fast
            if(current_right_motor_speed > current_right_max)
            {
                current_right_motor_speed = current_right_max;
            }
            else
            {
                current_right_motor_speed = current_right_motor_speed + error;
            }
        }
        else
        { 
            current_right_motor_speed = current_right_motor_speed - error;
            
            // make sure not going too fast
            if(current_left_motor_speed < current_left_max)
            {
                current_left_motor_speed = current_left_max;  
            }
            else
            {
                current_left_motor_speed = current_left_motor_speed - error;
            }
        }
        
        TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
    }