File content as of revision 4:a95f90a0410d:

/**
 ******************************************************************************
 * @file    main.cpp
 * @author  Julien Tiron, FIP Télécom Bretagne
 * @version V1.0.0
 * @date    March 23th, 2016
 * @brief   DoorCloser robot main code
 ******************************************************************************
 **/

/* Includes ------------------------------------------------------------------*/

#include "mbed.h"
#include "DevSPI.h"
#include "l6474_class.h"
#include "DevI2C.h"
#include "vl6180x_class.h"
#include "VL6180x.h"

/* Definitions ---------------------------------------------------------------*/

#define VL6180X_ADDRESS 0x29

/* Variables -----------------------------------------------------------------*/

/* Start and  Stop Component */

InterruptIn startup(PC_1);
Ticker game_length;
volatile bool start = 1;
volatile bool end = 1;
char data = 0x08 | (char)64;
bool tag = true;
int i2cAddres=0x70;                // Address of DS1307 is 0x68 (7 bit address)
int i2c8BitAddres= i2cAddres <<1;  // Convert to 8bit addressing used by mbed
volatile unsigned int step = 0;
/* Motor Control Component */
L6474 *motor1;
L6474 *motor2;

/* Distance Sensors Component */
//DevI2C *i2c =new DevI2C(D14, D15);
/*VL6180X sensor1(i2c);
VL6180X sensor2(i2c);
VL6180X sensor3(i2c);*/
I2C i2c(D14, D15);
VL6180x sensor(D14, D15, VL6180X_ADDRESS<<1);

/* Functions -----------------------------------------------------------------*/

void go()
{
    start = 0;
}

void stop()
{
    end = 0;
}

void init_sensor()
{

}

void switch_sensor(int number)
{

}

void deplacement(int distanceEnCm)
{
    int nbPas = distanceEnCm*160;
    motor1->Move(StepperMotor::BWD,nbPas);
    motor2->Move(StepperMotor::FWD,nbPas);
    motor1->WaitWhileActive();
    motor2->WaitWhileActive();
    motor1->HardStop();
    motor2->HardStop();
}

void rotationD(int angleEnDegre, int diametre)
{
    int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
    int nbPas = 160*deplacementEnCm;
    nbPas /= 500;

    /*
    int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14;
    int nbPas = 160*deplacementEnCm;
    nbPas *= 2;*/

    printf("%d \n\r", nbPas);
    motor2->Move(StepperMotor::FWD,nbPas);
    motor1->Move(StepperMotor::FWD,nbPas);
    motor1->WaitWhileActive();
    motor2->WaitWhileActive();
    motor1->HardStop();
    motor2->HardStop();
}
void rotationG(int angleEnDegre, int diametre)
{
    int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
    int nbPas = 160*deplacementEnCm;
    nbPas /= 500;

    /*
    int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14;
    int nbPas = 160*deplacementEnCm;
    nbPas *= 2;*/

    printf("%d \n\r", nbPas);
    motor2->Move(StepperMotor::BWD,nbPas);
    motor1->Move(StepperMotor::BWD,nbPas);
    motor1->WaitWhileActive();
    motor2->WaitWhileActive();
    motor1->HardStop();
    motor2->HardStop();
}

/* Main ----------------------------------------------------------------------*/

int main()
{
    /*----- Initialization. -----*/

    /* Initializing SPI bus. */
    DevSPI dev_spi(D11, D12, D13);

    /* Initializing Motor Control Components. */
    motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
    motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
    if (motor1->Init() != COMPONENT_OK)
        exit(EXIT_FAILURE);
    if (motor2->Init() != COMPONENT_OK)
        exit(EXIT_FAILURE);
    int result = i2c.write(i2c8BitAddres, &data, 1 );
    sensor.VL6180xDefautSettings();
    wait(1);
    /* Interrupt to start the robot */
    startup.fall(&go);


    while(start) {
        /* Waiting code */
        //deplacement(10);
        /*rotation(45, 9);
        wait(2);
        rotation(90,9);
        wait(2);
        rotation(45,9);
        wait(2);
        rotation(180,9);
        wait(2);
        rotation(45,9);
        start = 0;
        */
        /*
        motor1->Move(StepperMotor::FWD,nbPas);
        motor2->Move(StepperMotor::BWD,nbPas);
        motor1->WaitWhileActive();
        motor2->WaitWhileActive();*/
    }
    
    
    /* Interrupt to stop the robot */
    game_length.attach(&stop, 90); //1 minutes 30 secondes pour la Coupe
    
    
    while(end) {
        /* In-game code */


        switch (step) {

            case 0:

                wait(1);
                step = 1;
                break;

            case 1 :
                /* Avancer du tapis judqu'au mur */
                motor1->Run(StepperMotor::FWD);
                motor2->Run(StepperMotor::BWD);
                int dis;
                while(((dis = sensor.getDistance())>30)) {
                    //Tant que la distance est superieure on continue d'avancer
                    wait_ms(20);
                }
                printf("%d \n\r", dis);
//                motor1->Move(StepperMotor::FWD,1300);
//                motor2->Move(StepperMotor::BWD,1300);
//                motor1->WaitWhileActive();
//                motor2->WaitWhileActive();
                motor1->HardStop();
                motor2->HardStop();
                step = 2 ;
                break;

            case 2 : /* Angle 90° */
                rotationD(90,9); //rotation droite
                motor1->WaitWhileActive();
                motor2->WaitWhileActive();
                step = 3 ;
                break;

            case 3 : /* Fermeture de 2 portes : 60cm avant + arrêt à 3 cm du mur */
                motor1->Move(StepperMotor::FWD,7000); //160pas = 1cm
                motor2->Move(StepperMotor::BWD,7000);
                motor1->WaitWhileActive();
                motor2->WaitWhileActive();
                rotationG(180,9);
                deplacement(20);
                rotationD(90,9);
                deplacement(20);
                rotationD(90,9);
                deplacement(40);
                motor1->WaitWhileActive();
                motor2->WaitWhileActive();
                
                //160 pas = 1 cm
                //motor1->Run(StepperMotor::FWD);
                //motor2->Run(StepperMotor::BWD);
                while(sensor.getDistance()>30) {
                    //Tant que la distance est superieure on continue d'avancer
                    wait_ms(20);
                }
                motor1->HardStop();
                motor2->HardStop();
                step = 4;
                break;

            case 4 :
                // STOP : robot arrêté
                break;

        }

    }

    motor1->HardStop();
    motor2->HardStop();

    motor1->WaitWhileActive();
    motor2->WaitWhileActive();
}