File content as of revision 0:3e1d75bd313d:

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

*/



#include "mbed.h"

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
bool c; // memorise color so it knows which way to go at junction {true for red}
float white_reference,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 bool detect_color(int stage)
{
    leds=1;//turn on the 4 red leds
    if(stage==0)
    {
        white_reference=colorsensor.read();//global variable no prob
    }
    else
    {
        color=colorsensor.read();
        return (color> white_reference (+/-) threshold ? true:false); //true means red, false means blue
    }
}

int main() 
{
    //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);

}