File content as of revision 0:5121458888b5:

#include "mbed.h"

DigitalOut solenoid(PTC9);//mosfet gate controlling solenoid
PwmOut speed(PTC1);//speed for motors
PwmOut speedb(PTA4);
DigitalOut fwd1(PTA1);//directions for motors
DigitalOut back1(PTA2);
DigitalOut fwd2(PTA12);
DigitalOut back2(PTD4);
PwmOut red(PTA5);
PwmOut blue(PTC8);
AnalogIn sense(A0);
DigitalOut test_red(LED_RED);
DigitalOut test_blue(LED_BLUE);
Serial pc(PTE0,PTE1); // tx, rx
BusIn line(PTC3,PTC0,PTC7,PTC5,PTC4,PTC6,PTC10); //Line Board input
char command[256],c;
int num;
float color;
// Minimum and maximum motor speeds
#define MAX 0.9


// PID terms
#define P_TERM 30 
#define DD_TERM 0
#define D_TERM 76
#define k 0.1
inline float get_position()//-3 to 3 based on the sensor position 
{
    switch(line)//rates are wrong CHANGE ASAP
    {
        case 0x01:
        return -1.0;
        case 0x02:
        return -0.9;
        case 0x04:
        return -0.3;
        case 0x08:
        return 0;
        case 0x10:
        return 0.3;
        case 0x20:
        return 0.9;
        case 0x40:
        return 1.0;
    } 
}

void drive(float left,float right)
{
    if(left==right)
    {
        fwd1=1;back1=0;
        fwd2=1;back2=0;
    }
    if(left>right)
    {
        fwd1=1;back1=0;
        fwd2=0;back2=1;
    }
    if(left<right)
    {
    fwd1=0;back1=1;
    fwd2=1;back2=0;    
    }
    speed.write(left);
    speedb.write(right);

}
 

inline int detect_color()

{
       solenoid=1;
       red=0.75;blue=0;

       wait(0.01);

       

       color= sense.read();

        red=0;blue=0.75;wait(0.01);

        color=color - sense.read();

        red=0;blue=0;

        if(color >= 0.025 && color < 0.055)

        {

            //pc.printf("RED  %.3f\n\n\n\n",color);
            test_red=0;test_blue=1;
            return 0;

        }

        else if( color < 0.025 && color >= 0)

        {
            test_blue=0;test_red=1;
            return 1;

            //pc.printf("BLUE %.3f\n\n\n\n",color);

            }

        else{

            //pc.printf("Unknown Color \n\n\n\n");

            return 2;

            }

        //wait(1);

}

void command_mode()
{
    pc.printf("\n");
    while(1)
    {
        c=pc.getc();
    
        if(c=='p')//codename for PID indices
    
        {
    
            pc.printf("PID Coefficients Configuration\n");
            //pc.printf("kp %f, ki %f, kd %f",kp,ki,kd);
            c=pc.getc();
    
            num=(int)c-48;//get numerical value;
    
            switch(num)
            {
    
                case 1:
    
    
                break;
    
                case 2:
    
    
                break;
    
                case 3: 
    
               
    
                break;
    
                default:
    
                break;
    
            }
    
                pc.printf("Done!\n");
    
            memset(command,NULL, sizeof(command));
            
    
        }
    
        else if(c=='c')//codename for color detection
    
        {
            int col;
            col=detect_color();
    
            pc.printf("%d\n",col);
    
        }
    
        else if(c=='l')//codename for line detection 
    
        {    
            pc.printf("%f\n",get_position());//placeholder code 
    
        }
        else if(c=='s')
        {
               if((int)solenoid.read() == 1)
                {
                    solenoid=0;
                    pc.printf("Solenoid disengaged\n");
                }
                else if ((int)solenoid.read() ==0)
                {
                    solenoid = 1;
                    pc.printf("Solenoid engaged\n");
                }
                
        }
    
        else
        {
            pc.printf("Command Unknown.\n");
        }
}
} 


int main() 
{
    wait(2);
    if(pc.readable())
    {
        if((char)pc.getc()=='z');
        command_mode();
    }
    else
    {

    wait(2.0);

    float right;
    float left;
    float current_pos_of_line = 0.0;
    float previous_pos_of_line = 0.0;
    float previous_derivative=0;
    float derivative,proportional,DoD = 0;
    float power;
    float speed = MAX;
    solenoid=1;
    while (1) {
        detect_color();
        // Get the position of the line.
        current_pos_of_line = get_position();      
        proportional = current_pos_of_line;
        
        // Compute the derivative
        derivative = current_pos_of_line - previous_pos_of_line;
        
        // Compute the DoD
        DoD = derivative - previous_derivative;
        
        // Remember the last position.
        previous_pos_of_line = current_pos_of_line;
        previous_derivative = derivative;
        // Compute the power
        power = k*(proportional * (P_TERM) ) + (DoD*(DD_TERM)) + (derivative*(D_TERM)) ;
        
        
    

        // Compute new speeds   
        right = speed+power;
       left  = speed-power;
            
       // set speed 
        drive(left,right);
    }
    }
}