File content as of revision 4:0e9f65e3e2a0:

/**
 ******************************************************************************
 * @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.h"
#include "AX12.h"
#include "Servo.h"
/* Definitions ---------------------------------------------------------------*/

#define VL6180X_ADDRESS 0x29

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

/* Start and  Stop Component */
InterruptIn startup(PC_1);
Ticker pwm_ticker;
Timeout game_length;
volatile bool start = 1;
volatile bool end = 1;
char data = 0x08 | (char)64;
int step=0;
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
Servo myservo(D5);
PwmOut parasol(A3);
/* Motor Control Component */
L6474 *motor1;
L6474 *motor2;
DigitalInOut sdaDummy(D14);
DigitalInOut sclDummy(D15);
/* Distance Sensors Component */
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 pwm();

void deplacement(int distanceEnCm, bool sens)
{
    int nbPas = distanceEnCm*166.7;
    if(sens) {
        motor1->Move(StepperMotor::BWD,nbPas);
        motor2->Move(StepperMotor::FWD,nbPas);
    } else {
        motor1->Move(StepperMotor::FWD,nbPas);
        motor2->Move(StepperMotor::BWD,nbPas);
    }
    /*while((sensor.getDistance()>50) && end) {
        //Tant que la distance est superieure on continue d'avancer
        wait_ms(20);
        printf("1");
    }*/
    motor1->WaitWhileActive();
    motor2->WaitWhileActive();
    motor1->HardStop();
    motor2->HardStop();
}


void rotation(int angleEnDegre, int diametre, int sens)
{
    int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
    int nbPas = 160*deplacementEnCm;
    nbPas /= 990;

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

    printf("%d \n\r", nbPas);
    if(sens) {
        motor2->Move(StepperMotor::FWD,nbPas);
        motor1->Move(StepperMotor::FWD,nbPas);
    } else {
        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);
    myservo.calibrate(0.00095, 90.0); // Calibrate the servo
    parasol.period_ms(10);
    parasol.pulsewidth_ms(1);
    /* 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);
    sdaDummy.mode(PullUp);
    sclDummy.mode(PullUp);
    //pwm_ticker.attach(&pwm, 0.1);
    /* Initializing AX-12 motors */
    /*    AX12 ax12_1 (D1, D0, 1, 1000000);
        AX12 ax12_2 (D1, D0, 2, 1000000);
        AX12 ax12_3 (D1, D0, 3, 1000000);
        AX12 ax12_4 (D1, D0, 4, 1000000);
        AX12 ax12_5 (D1, D0, 5, 1000000);*/

    /* Interrupt to start the robot */
    startup.fall(&go);
    /*motor1->Move(StepperMotor::BWD,6000);
    motor2->Move(StepperMotor::FWD,6000);
    motor1->WaitWhileActive();
    motor2->WaitWhileActive();
    motor1->HardStop();
    motor2->HardStop();*/
    //deplacement(10, 0);
    /*    rotation(90, 30);
        wait(2);
        rotation(45, 30);
        wait(2);
        rotation(180, 30);
        wait(2);
        rotation(45, 30);*/
    /*motor1->Move(StepperMotor::FWD,6000);
    motor2->Move(StepperMotor::BWD,6000);
    motor1->WaitWhileActive();
    motor2->WaitWhileActive();
    motor1->HardStop();
    motor2->HardStop();
    */

    while(start) {
        /* Waiting code */
        /*        ax12_1.SetSpeed(70);
                ax12_2.SetSpeed(70);
                ax12_3.SetSpeed(70);
                ax12_4.SetSpeed(70);
                ax12_5.SetSpeed(70);*/
        //wait_ms(100);
    }

    /* Interrupt to stop the robot */
    game_length.attach(&stop, 90); //1 minutes 30 secondes pour la Coupe

    while(end) {

        switch(step) {
            case 0:
                step = 1;
                break;

            case 1:
                //Déplacement 1ers cubes au milieu
                printf("deplacement init \n");
                wait_ms(100);
                deplacement(90,1);

                step = 2;
                break;

            case 2:
                //Mise en place pour pêche
                //motor1->Move(StepperMotor::FWD,4000); //première rotation 90°
//                motor2->Move(StepperMotor::FWD,4000);
//                motor1->WaitWhileActive();
//                motor2->WaitWhileActive();
//                motor1->HardStop();
//                motor2->HardStop();
//                printf("Rotation 1 \n");
//
//
//                motor1->Move(StepperMotor::BWD,6000); //Avance jusqu'au mur
//                motor2->Move(StepperMotor::FWD,6000);
//                motor1->WaitWhileActive();
//                motor2->WaitWhileActive();
//                motor1->HardStop();
//                motor2->HardStop();
//
//                motor1->Move(StepperMotor::FWD,600); //recul pour rotation
//                motor2->Move(StepperMotor::BWD,600);
//                motor1->WaitWhileActive();
//                motor2->WaitWhileActive();
//                motor1->HardStop();
//                motor2->HardStop();
//                printf("Recul Rotation 2\n");
//
//
//
//                motor1->Move(StepperMotor::FWD,4000); //deuxième rotation 90° (même sens que première)
//                motor2->Move(StepperMotor::FWD,4000);
//                motor1->WaitWhileActive();
//                motor2->WaitWhileActive();
//                motor1->HardStop();
//                motor2->HardStop();
//                printf("Rotation 2 \n");
//
//
//                motor1->Move(StepperMotor::BWD,6000); //mise en position
//                motor2->Move(StepperMotor::FWD,6000);
//                motor1->WaitWhileActive();
//                motor2->WaitWhileActive();
//                motor1->HardStop();
//                motor2->HardStop();
//                printf("Mise en position\n");
//
//                motor2->Move(StepperMotor::BWD,4000); //troisième rotation 90°
//                motor1->Move(StepperMotor::BWD,4000);
//                motor1->WaitWhileActive();
//                motor2->WaitWhileActive();
//                motor1->HardStop();
//                motor2->HardStop();
//                printf("Rotation 3 \n");
//
//
//                motor2->Move(StepperMotor::BWD,500); //Collage mur
//                motor1->Move(StepperMotor::FWD,500);
//                motor1->WaitWhileActive();
//                motor2->WaitWhileActive();
//                motor1->HardStop();
//                motor2->HardStop();
//                printf("Collage\n");
//
//
//                motor2->Move(StepperMotor::FWD,600); //recul pour rotation
//                motor1->Move(StepperMotor::BWD,600);
//                motor1->WaitWhileActive();
//                motor2->WaitWhileActive();
//                motor1->HardStop();
//                motor2->HardStop();
//
//                motor2->Move(StepperMotor::BWD,4000); //quatrième rotation 90°
//                motor1->Move(StepperMotor::BWD,4000);
//                motor1->WaitWhileActive();
//                motor2->WaitWhileActive();
//                motor1->HardStop();
//                motor2->HardStop();
//                printf("Rotation 4 \n");
                printf("deplacement arriere 40 \n");
                deplacement(40,0);
                printf("rotation 90 1 \n");
                rotation(90, 30, 0);
                printf("Rotation 1 \n");
                printf("deplacement coller mur\n");
                deplacement(112,1);
                printf("deplacement arriere\n");
                deplacement(3,0);
                printf("rotation finale\n");
                rotation(90, 30, 1);
                printf("avancement 40\n");
                deplacement(40,1);

                //Mise en position du robot pour pêche

                step = 3;
                break;
            case 3:
                //déploiement des bras, pêche

                step = 4;
                break;
            case 4:
                //Dépose des poissons dans le filet

                step = 5;

            case 5:
                break;


        }
    }

    parasol.write(0.5);
    printf("2");
    motor1->HardStop();
    motor2->HardStop();
}

void pwm()
{
}