File content as of revision 18:96264a9ddaf6:

#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       right_center      (PC_3);
CNY70       left_center      (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 = -200, speed_R = -200;
    bool reset = false;
    while (1) {

        motor.getPosition(&x, &y, &theta);
        pc.printf("\r state is %d x is %.2lf, y is %.2lf, theta is %.2lf",current_state,x,y,theta);
        switch (current_state) {
            case IDLE :
                motor.setSpeed(0,0);
                if (bp == 0) current_state = TURN_RIGHT;
                break;

            case TURN_RIGHT :
                motor.setSpeed(-speed_L/10.0,speed_R/10.0);// turn right

                if (pixy.checkNewImage()) {
                    pixy.detectedObject(&numberOfObjects,&dummy);
                    if (numberOfObjects==1) {
                        current_state = LOOK_FOR_FILE;
                    }
                }
                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 (left_center.whatAmIOn()== 0 && reset == false)
                {
                    led1==1;
                    motor.resetPosition();
                    reset = true;
                    
                }
                if (x<=-1300) current_state = STUPID1;
                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/10.0,speed_R/10.0);// turn right
                            pc.printf(" - Turning right");
                        } else if (file.x <155) {
                            motor.setSpeed(speed_L/10.0,-speed_R/10.0);// turn left
                            pc.printf(" - Turning left");
                        } else {
                            motor.setSpeed(0,0);
                            motor.resetPosition();
                            current_state = FORWARD;
                        }
                    } else {
                        //motor.setSpeed(-speed_L,speed_R);// turn right
                        pc.printf(" - No object");
                    }
                }
                break;
                
                case STUPID1 :
                    wait(0.2);
                    current_state = HARVEST1;
                    break;
                    
                case STUPID2 :
                    wait(0.2);
                    current_state = HARVEST2;
                    break;
                    
                case STUPID3 :
                    wait(0.2);
                    current_state = HARVEST3;
                    break;
                    
                
                
                case HARVEST1 :
                    motor.setSpeed(-speed_L,0.0);
                    if (theta < (-2*PI)) current_state = STUPID2;
                break;

                case HARVEST2 :
                    motor.setSpeed(0.0,-speed_R);
                    if (theta > 0) current_state = STUPID3;
                break;

                case HARVEST3 :
                    motor.setSpeed(speed_L,-speed_R);
                    if (theta > (2*PI)) current_state = IDLE;
                break;
 
             default :
                //do something
                break;
        }
    }

    return 0;
}