File content as of revision 1:a8c5e92c6cbf:

// Initialize a pins to perform Serial Communication for receive
// the result of the printf on PC (USB Virtual Com)
// I suggest to use TeraTerm on PC.
// TeraTerm configuration must be: 9600-8-N-1 FlowControl None

/* EXAMPLE */
#include "mbed.h"
#include "hcsr04.h"

Serial pc(USBTX, USBRX);


DigitalOut linksachter(D9);
DigitalOut rechtsachter(A6);
DigitalOut rechtsvoor(D7);
DigitalOut linksvoor(D10);
PwmOut pwm(D6); // links
PwmOut pwm2(D11);

HCSR04 sensor(D4, A0);



DigitalIn lijnsensor4(D1);
DigitalIn lijnsensor3(D0);
DigitalIn lijnsensor1(D2);
DigitalIn lijnsensor2(D3);


HCSR04 sensor2(D4, A2);
//HCSR04 sensor2(D4, A0);
//HCSR04 sensor3(D4, A0);
//HCSR04 sensor4(D4, A0);



Timer timer;
Timer timer2;
Timer timer3;
Timer timer4;
Timer timer5;
Timer timer6;


enum ROBOTSTATE {vooruit, lijn_detect_linksvoor, lijn_detect_rechtsvoor, lijn_detect_linksachter, lijn_detect_rechtsachter, lijn_detect_voor, lijn_detect_achter, robot_detect_voor, robot_detect_rechts, robot_detect_links, robot_detect_achter, start,};
ROBOTSTATE myState;
int main()
{
     pwm.period_ms(10/6);
    pwm.pulsewidth_ms(1);
    pwm2.period_ms(10/6);
    pwm2.pulsewidth_ms(1);

    while(1) {
        
          

       switch(myState) {

  

    

            case vooruit:
              pwm.period_ms(10/6);
    pwm.pulsewidth_ms(1);
    pwm2.period_ms(10/6);
    pwm2.pulsewidth_ms(1);
            
                pwm=0.4;
                pwm2=0.4;
                linksvoor=1;
                rechtsvoor=1;
                linksachter=0;
                rechtsachter=0;
                if(lijnsensor1==0) {
                    myState = lijn_detect_linksvoor;
                }
                if(lijnsensor2==0) {
                    myState=lijn_detect_rechtsvoor;
                }
                if (lijnsensor3 ==0) {
                    myState=lijn_detect_linksachter;
                }
                if (lijnsensor4==0) {
                    myState = lijn_detect_rechtsachter;
                }
                if (sensor.distance()<50) { myState = robot_detect_voor; }
                if (sensor2.distance()<30) {myState = robot_detect_achter;}
           break;
               
             

            case lijn_detect_linksvoor:
  
                linksvoor=1;
                rechtsvoor=0;
                linksachter=0;
                rechtsachter=0;
                pwm=0.9;
                pwm2=0.9;
                wait_ms(2000);
                linksvoor=1;
                rechtsvoor=1;
                wait_ms(500);
                myState = vooruit;
                break;

            case lijn_detect_rechtsvoor:
     

                linksvoor=0;
                rechtsvoor=1;
                linksachter=0;
                rechtsachter=0;
                pwm=0.9;
                pwm2=0.9;
                wait_ms(2000);
                linksvoor=1;
                rechtsvoor=1;
                wait_ms(500);
                myState= vooruit;
                break;


            case lijn_detect_linksachter:
   
                timer3.start();
                linksvoor=0;
                rechtsvoor=1;
                linksachter=0;
                rechtsachter=0;
                pwm=0.7;
                pwm2=0.8;
                if(timer3.read_ms() >= 500 ) {
                    timer3.stop();
                    timer3.reset();
                    myState = vooruit;
                }
                break;

            case lijn_detect_rechtsachter:
    
                timer4.start();
                linksvoor=1;
                rechtsvoor=0;
                linksachter=0;
                rechtsachter=0;
                pwm=0.7;
                pwm2=0.8;
                if(timer4.read_ms() >= 500 ) {   // voor twee seconden draait hij
                    timer4.stop();
                    timer4.reset();
                    myState = vooruit;
                }
                break;

        
            case robot_detect_voor:
 
              
            timer5.start();
            linksvoor=1;
            rechtsvoor=1;
            linksachter=0;
            rechtsachter=0;
            pwm=1;
            pwm2=1;
            
             if(lijnsensor1==0) {
                    timer5.stop(); timer5.reset();  myState = lijn_detect_linksvoor;
                }
                if(lijnsensor2==0) {
                   timer5.stop(); timer5.reset();  myState=lijn_detect_rechtsvoor;
                }
                if (lijnsensor3 ==0) {
                   timer5.stop(); timer5.reset();  myState=lijn_detect_linksachter;
                }
                if (lijnsensor4==0) {
                    timer5.stop(); timer5.reset();  myState = lijn_detect_rechtsachter;
                }
            if (timer5.read_ms() >= 2000) { timer5.stop(); timer5.reset(); myState = vooruit; }
            break;

    case robot_detect_achter:


         timer6.start();
         pwm=1;
         pwm2=1;
            linksvoor=1;
            rechtsvoor=0;
           linksachter=0;
           rechtsachter=0;
           
             if(lijnsensor1==0) {
                    timer6.stop(); timer6.reset();  myState = lijn_detect_linksvoor;
              }
              if(lijnsensor2==0) {
                 timer6.stop(); timer6.reset();  myState=lijn_detect_rechtsvoor;
              }
             if (lijnsensor3 ==0) {
                  timer6.stop(); timer6.reset();  myState=lijn_detect_linksachter;
               }
               if (lijnsensor4==0) {
                   timer6.stop(); timer6.reset();  myState = lijn_detect_rechtsachter;
                }
            if (timer6.read_ms() >= 1000) { timer6.stop(); timer6.reset(); myState = vooruit; }
   
           






        }
    }
}