File content as of revision 12:548cdc49cdba:

#include "mbed.h"
#include "CMPS03.h"
#include "CNY70.h"
#include "GP2A.h"
#include "VMA306.h"
#include "Pixy.h"
#include "PID.h"
#include "motor_state.h"

#define PI  3.1415926535898

Serial      pc          (PA_2, PA_3, 115200);

PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);

CMPS03      boussole    (PC_4);

CNY70       ligneD      (PC_3);
CNY70       ligneG      (PC_2);
CNY70       exterior    (PA_7);

VMA306      ultraSon    (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);

PIXY        pixy        (PA_0, PA_1, 50000);

DigitalIn   bp          (PC_13);

DigitalOut  led1        (PA_5);
DigitalOut  led2        (PD_2);
DigitalOut  unused1     (PB_10);
DigitalOut  unused2     (PA_15);
DigitalOut  unused3     (PA_12);

DigitalIn   unused4     (PA_4, PullUp);
DigitalIn   unused5     (PB_0, PullUp);
DigitalIn   unused6     (PC_1, PullUp);
DigitalIn   unused7     (PC_0, PullUp);

Timer       temps;

motor_state current_state = IDLE;

int main ()
{
    int numberOfObjects=0, dummy=0;
    T_pixyNMBloc file; 
    motor.resetPosition();
    double x=0, y=0, theta = 0;
    motor.setProportionnalValue(2.0);
    motor.setintegralValue(0.4);
    motor.setDerivativeValue(1.0);
    double speed_L = -50, speed_R = -50;
    while (1) {
        
        motor.getPosition(&x, &y, &theta);
        pc.printf("\r x is %.2lf, y is %.2lf, theta is %.2lf",x,y,theta);
        switch (current_state) 
        {
        case IDLE : 
            motor.setSpeed(0,0);
            if (bp == 0) current_state = LOOK_FOR_FILE;
            break;
         
         case TURN_RIGHT :
            motor.setSpeed(speed_L,-speed_R);
            
            if (theta <= -PI/3) {
                motor.resetPosition();
                current_state = FORWARD;
                }
            break;   
            
          case SAFEMODE :
            motor.setSpeed(0,0);
            if (ultraSon.readUSB() >= 20) current_state = FORWARD;
            if (ultraSon.readUSB() <= 15) current_state = BACKWARD;
            break;            
            
        case FORWARD : 
                motor.setSpeed(speed_L,speed_R);
                if (ultraSon.readUSB() <= 20) {
                    current_state = SAFEMODE;
                }
                if (x<=-2000) current_state = IDLE;
            break;
            
        case BACKWARD : 
                motor.setSpeed(-speed_L,-speed_R);
                if (ultraSon.readUSB() >= 15) current_state = SAFEMODE;
            break;
            
        case LOOK_FOR_FILE :               
           if (pixy.checkNewImage()) {
            pixy.detectedObject(&numberOfObjects,&dummy);
            if (numberOfObjects==1) {
                file=pixy.getNMBloc();
                pc.printf("\r x = %d", file.x);
                if (file.x >165){
                    motor.setSpeed(-speed_L,speed_R);// turn right
                    pc.printf(" - Turning right");
                }
                else if (file.x <155){
                    motor.setSpeed(speed_L,-speed_R);// turn left
                    pc.printf(" - Turning left");
                }
                else {
                    motor.setSpeed(0,0);
                    current_state = FORWARD;
                }
            }
            else {
                motor.setSpeed(-speed_L,speed_R);// turn right
                pc.printf(" - No object");
                }
            }
            else 
                {
                    numberOfObjects = 0;
                    //motor.setSpeed(-speed_L,speed_R);// turn right
                   // motor.setSpeed(0,0);    
            } // end of pixy.checkNewImage
                              
            break;
    
        default : 
            //do something
            break;
        }
    }
    
    return 0;
}