main.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    main.cpp
00004  * @author  Julien Tiron, FIP Télécom Bretagne
00005  * @version V1.0.0
00006  * @date    March 23th, 2016
00007  * @brief   DoorCloser robot main code
00008  ******************************************************************************
00009  **/
00010 
00011 /* Includes ------------------------------------------------------------------*/
00012 
00013 #include "mbed.h"
00014 #include "DevSPI.h"
00015 #include "l6474_class.h"
00016 #include "DevI2C.h"
00017 #include "vl6180x_class.h"
00018 #include "VL6180x.h"
00019 
00020 /* Definitions ---------------------------------------------------------------*/
00021 
00022 #define VL6180X_ADDRESS 0x29
00023 
00024 /* Variables -----------------------------------------------------------------*/
00025 
00026 /* Start and  Stop Component */
00027 
00028 InterruptIn startup(PC_1);
00029 Ticker game_length;
00030 volatile bool start = 1;
00031 volatile bool end = 1;
00032 char data = 0x08 | (char)64;
00033 bool tag = true;
00034 int i2cAddres=0x70;                // Address of DS1307 is 0x68 (7 bit address)
00035 int i2c8BitAddres= i2cAddres <<1;  // Convert to 8bit addressing used by mbed
00036 volatile unsigned int step = 0;
00037 /* Motor Control Component */
00038 L6474 *motor1;
00039 L6474 *motor2;
00040 
00041 /* Distance Sensors Component */
00042 //DevI2C *i2c =new DevI2C(D14, D15);
00043 /*VL6180X sensor1(i2c);
00044 VL6180X sensor2(i2c);
00045 VL6180X sensor3(i2c);*/
00046 I2C i2c(D14, D15);
00047 VL6180x sensor(D14, D15, VL6180X_ADDRESS<<1);
00048 
00049 /* Functions -----------------------------------------------------------------*/
00050 
00051 void go()
00052 {
00053     start = 0;
00054 }
00055 
00056 void stop()
00057 {
00058     end = 0;
00059 }
00060 
00061 void init_sensor()
00062 {
00063 
00064 }
00065 
00066 void switch_sensor(int number)
00067 {
00068 
00069 }
00070 
00071 void deplacement(int distanceEnCm)
00072 {
00073     int nbPas = distanceEnCm*160;
00074     motor1->Move(StepperMotor::BWD,nbPas);
00075     motor2->Move(StepperMotor::FWD,nbPas);
00076     motor1->WaitWhileActive();
00077     motor2->WaitWhileActive();
00078     motor1->HardStop();
00079     motor2->HardStop();
00080 }
00081 
00082 void rotationD(int angleEnDegre, int diametre)
00083 {
00084     int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
00085     int nbPas = 160*deplacementEnCm;
00086     nbPas /= 500;
00087 
00088     /*
00089     int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14;
00090     int nbPas = 160*deplacementEnCm;
00091     nbPas *= 2;*/
00092 
00093     printf("%d \n\r", nbPas);
00094     motor2->Move(StepperMotor::FWD,nbPas);
00095     motor1->Move(StepperMotor::FWD,nbPas);
00096     motor1->WaitWhileActive();
00097     motor2->WaitWhileActive();
00098     motor1->HardStop();
00099     motor2->HardStop();
00100 }
00101 void rotationG(int angleEnDegre, int diametre)
00102 {
00103     int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
00104     int nbPas = 160*deplacementEnCm;
00105     nbPas /= 500;
00106 
00107     /*
00108     int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14;
00109     int nbPas = 160*deplacementEnCm;
00110     nbPas *= 2;*/
00111 
00112     printf("%d \n\r", nbPas);
00113     motor2->Move(StepperMotor::BWD,nbPas);
00114     motor1->Move(StepperMotor::BWD,nbPas);
00115     motor1->WaitWhileActive();
00116     motor2->WaitWhileActive();
00117     motor1->HardStop();
00118     motor2->HardStop();
00119 }
00120 
00121 /* Main ----------------------------------------------------------------------*/
00122 
00123 int main()
00124 {
00125     /*----- Initialization. -----*/
00126 
00127     /* Initializing SPI bus. */
00128     DevSPI dev_spi(D11, D12, D13);
00129 
00130     /* Initializing Motor Control Components. */
00131     motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
00132     motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
00133     if (motor1->Init() != COMPONENT_OK)
00134         exit(EXIT_FAILURE);
00135     if (motor2->Init() != COMPONENT_OK)
00136         exit(EXIT_FAILURE);
00137     int result = i2c.write(i2c8BitAddres, &data, 1 );
00138     sensor.VL6180xDefautSettings();
00139     wait(1);
00140     /* Interrupt to start the robot */
00141     startup.fall(&go);
00142 
00143 
00144     while(start) {
00145         /* Waiting code */
00146         //deplacement(10);
00147         /*rotation(45, 9);
00148         wait(2);
00149         rotation(90,9);
00150         wait(2);
00151         rotation(45,9);
00152         wait(2);
00153         rotation(180,9);
00154         wait(2);
00155         rotation(45,9);
00156         start = 0;
00157         */
00158         /*
00159         motor1->Move(StepperMotor::FWD,nbPas);
00160         motor2->Move(StepperMotor::BWD,nbPas);
00161         motor1->WaitWhileActive();
00162         motor2->WaitWhileActive();*/
00163     }
00164     
00165     
00166     /* Interrupt to stop the robot */
00167     game_length.attach(&stop, 90); //1 minutes 30 secondes pour la Coupe
00168     
00169     
00170     while(end) {
00171         /* In-game code */
00172 
00173 
00174         switch (step) {
00175 
00176             case 0:
00177 
00178                 wait(1);
00179                 step = 1;
00180                 break;
00181 
00182             case 1 :
00183                 /* Avancer du tapis judqu'au mur */
00184                 motor1->Run(StepperMotor::FWD);
00185                 motor2->Run(StepperMotor::BWD);
00186                 int dis;
00187                 while(((dis = sensor.getDistance())>30)) {
00188                     //Tant que la distance est superieure on continue d'avancer
00189                     wait_ms(20);
00190                 }
00191                 printf("%d \n\r", dis);
00192 //                motor1->Move(StepperMotor::FWD,1300);
00193 //                motor2->Move(StepperMotor::BWD,1300);
00194 //                motor1->WaitWhileActive();
00195 //                motor2->WaitWhileActive();
00196                 motor1->HardStop();
00197                 motor2->HardStop();
00198                 step = 2 ;
00199                 break;
00200 
00201             case 2 : /* Angle 90° */
00202                 rotationD(90,9); //rotation droite
00203                 motor1->WaitWhileActive();
00204                 motor2->WaitWhileActive();
00205                 step = 3 ;
00206                 break;
00207 
00208             case 3 : /* Fermeture de 2 portes : 60cm avant + arrêt à 3 cm du mur */
00209                 motor1->Move(StepperMotor::FWD,7000); //160pas = 1cm
00210                 motor2->Move(StepperMotor::BWD,7000);
00211                 motor1->WaitWhileActive();
00212                 motor2->WaitWhileActive();
00213                 rotationG(180,9);
00214                 deplacement(20);
00215                 rotationD(90,9);
00216                 deplacement(20);
00217                 rotationD(90,9);
00218                 deplacement(40);
00219                 motor1->WaitWhileActive();
00220                 motor2->WaitWhileActive();
00221                 
00222                 //160 pas = 1 cm
00223                 //motor1->Run(StepperMotor::FWD);
00224                 //motor2->Run(StepperMotor::BWD);
00225                 while(sensor.getDistance()>30) {
00226                     //Tant que la distance est superieure on continue d'avancer
00227                     wait_ms(20);
00228                 }
00229                 motor1->HardStop();
00230                 motor2->HardStop();
00231                 step = 4;
00232                 break;
00233 
00234             case 4 :
00235                 // STOP : robot arrêté
00236                 break;
00237 
00238         }
00239 
00240     }
00241 
00242     motor1->HardStop();
00243     motor2->HardStop();
00244 
00245     motor1->WaitWhileActive();
00246     motor2->WaitWhileActive();
00247 }