File content as of revision 1:d9fb4f6b30f7:

/*
TODO:
Line following, gaussian filter, 
Color det integration 
PID algorithm integration 
Line following integration
Calibration????

*/



#include "mbed.h"

InterruptIn sw1(D10);

Ticker cmd;
char command[256],c;
int num;


DigitalOut solenoid(D7);//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);
DigitalOut leds(PTC8);// the 4 red leds
int col; // memorise color so it knows which way to go at junction {true for red}
PwmOut red(PTC9);
PwmOut blue(PTC8);
AnalogIn sense(A0);
Serial pc(USBTX,USBRX);
float color;
float spd1,spd2;//speeds of left and right motors


void solenoid_engage()
{
    solenoid=1;
}

void solenoid_off()
{
    solenoid=0;
}

/*inline void move(spd1,spd2)
{
    {speed1>0 ? fwd1=1;back1=0 : fwd1=0;back1=1};
    {speed2>0 ? fwd2=1;back2=0 : fwd2=0;back2=1};
}
*/
inline int detect_color()
{
       red=0.75;blue=0;
       wait(0.3);
       
       color= sense.read();
        red=0;blue=0.75;wait(0.3);
        color=color - sense.read();
        red=0;blue=0;
        if(color >= 0.025 && color < 0.055)
        {
            //pc.printf("RED  %.3f\n\n\n\n",color);
            return 0;
        }
        else if( color < 0.025 && color >= 0)
        {
            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()
{
    c=pc.getc();
    if(c=='p')//codename for PID indices
    {
        pc.printf("PID Coefficients Configuration\n");
        c=pc.getc();
        //c=pc.scanf("%s\n",&command);
        num=(int)c-48;//get numerical value;
        /*switch(num)
        {
            case 1:
            myled=1;
            break;
            case 2:
            myled2=1;
            break;
            case 3: 
            myled3=1;
            break;
            default:
            myled=0;myled2=0;myled3=0;break;
        }*/
            pc.printf("Done!\n");
        memset(command,NULL, sizeof(command));
    
        
    }
    else if(c=='c')//codename for color detection
    {
        col=detect_color();
        pc.printf("%d",col);
    }
    else if(c=='l')//codename for line detection 
    {
        //detect line and send stuff over serial -> hex number? could be too big to handle on script side 
        pc.printf("112\n");//placeholder code 
    }
    else
    {
        pc.printf("Command Unknown.\n");
    }
}

        

int x=0;
void data(){
    if(x>20) x=0;
    pc.printf("%d\n",x);
    x++;
    if(sw1==1) data();
    else 
    {
        pc.printf("%d\n",-1);
        return;
        }
}



int main() 
{
    sw1.rise(&command_mode);
    //pwm driver code will go into a function asap
    /*speed.period(0.2f);
    speedb.period(0.2f);

    speed.write(0.0);speedb.write(0.0);//do not move and scan color
    float i=0.5,j=0.5;

    
           fwd1=0;back1=1;//fwd
           fwd2=1;back2=0;
           speed.write(i);
           speedb.write(j);
           wait(2);
        
           
          fwd1=0;back1=1;//back
           fwd2=0;back2=1;
            speed.write(0);
            speedb.write(0);
           wait(2);*/

}