Yannick OMNES / Mbed 2 deprecated 2-FisherMan

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   AX12 Servo VL6180x X_NUCLEO_COMMON X_NUCLEO_IHM01A1 mbed

Fork of 2-FisherMan by Julien Tiron

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

Go to the documentation of this file.
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.h"
00018 #include "AX12.h"
00019 #include "Servo.h"
00020 /* Definitions ---------------------------------------------------------------*/
00021 
00022 #define VL6180X_ADDRESS 0x29
00023 
00024 /* Variables -----------------------------------------------------------------*/
00025 
00026 /* Start and  Stop Component */
00027 InterruptIn startup(PC_1);
00028 Ticker pwm_ticker;
00029 Timeout game_length;
00030 volatile bool start = 1;
00031 volatile bool end = 1;
00032 char data = 0x08 | (char)64;
00033 int step=0;
00034 bool tag = true;
00035 int i2cAddres=0x70;                // Address of DS1307 is 0x68 (7 bit address)
00036 int i2c8BitAddres= i2cAddres <<1;  // Convert to 8bit addressing used by mbed
00037 Servo myservo(D5);
00038 PwmOut parasol(A3);
00039 /* Motor Control Component */
00040 L6474 *motor1;
00041 L6474 *motor2;
00042 DigitalInOut sdaDummy(D14);
00043 DigitalInOut sclDummy(D15);
00044 /* Distance Sensors Component */
00045 VL6180x sensor(D14, D15, VL6180X_ADDRESS<<1);
00046 
00047 /* Functions -----------------------------------------------------------------*/
00048 
00049 void go()
00050 {
00051     start = 0;
00052 }
00053 
00054 void stop()
00055 {
00056     end = 0;
00057 }
00058 
00059 void init_sensor()
00060 {
00061 
00062 }
00063 
00064 void switch_sensor(int number)
00065 {
00066 
00067 }
00068 
00069 void pwm();
00070 
00071 void deplacement(int distanceEnCm, bool sens)
00072 {
00073     int nbPas = distanceEnCm*166.7;
00074     if(sens) {
00075         motor1->Move(StepperMotor::BWD,nbPas);
00076         motor2->Move(StepperMotor::FWD,nbPas);
00077     } else {
00078         motor1->Move(StepperMotor::FWD,nbPas);
00079         motor2->Move(StepperMotor::BWD,nbPas);
00080     }
00081     /*while((sensor.getDistance()>50) && end) {
00082         //Tant que la distance est superieure on continue d'avancer
00083         wait_ms(20);
00084         printf("1");
00085     }*/
00086     motor1->WaitWhileActive();
00087     motor2->WaitWhileActive();
00088     motor1->HardStop();
00089     motor2->HardStop();
00090 }
00091 
00092 
00093 void rotation(int angleEnDegre, int diametre, int sens)
00094 {
00095     int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
00096     int nbPas = 160*deplacementEnCm;
00097     nbPas /= 990;
00098 
00099     /*
00100     int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14;
00101     int nbPas = 160*deplacementEnCm;
00102     nbPas *= 2;*/
00103 
00104     printf("%d \n\r", nbPas);
00105     if(sens) {
00106         motor2->Move(StepperMotor::FWD,nbPas);
00107         motor1->Move(StepperMotor::FWD,nbPas);
00108     } else {
00109         motor2->Move(StepperMotor::BWD,nbPas);
00110         motor1->Move(StepperMotor::BWD,nbPas);
00111 
00112     }
00113     motor1->WaitWhileActive();
00114     motor2->WaitWhileActive();
00115     motor1->HardStop();
00116     motor2->HardStop();
00117 }
00118 
00119 
00120 /* Main ----------------------------------------------------------------------*/
00121 
00122 int main()
00123 {
00124     /*----- Initialization. -----*/
00125 
00126     /* Initializing SPI bus. */
00127     DevSPI dev_spi(D11, D12, D13);
00128     myservo.calibrate(0.00095, 90.0); // Calibrate the servo
00129     parasol.period_ms(10);
00130     parasol.pulsewidth_ms(1);
00131     /* Initializing Motor Control Components. */
00132     motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
00133     motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
00134     if (motor1->Init() != COMPONENT_OK)
00135         exit(EXIT_FAILURE);
00136     if (motor2->Init() != COMPONENT_OK)
00137         exit(EXIT_FAILURE);
00138     sdaDummy.mode(PullUp);
00139     sclDummy.mode(PullUp);
00140     //pwm_ticker.attach(&pwm, 0.1);
00141     /* Initializing AX-12 motors */
00142     /*    AX12 ax12_1 (D1, D0, 1, 1000000);
00143         AX12 ax12_2 (D1, D0, 2, 1000000);
00144         AX12 ax12_3 (D1, D0, 3, 1000000);
00145         AX12 ax12_4 (D1, D0, 4, 1000000);
00146         AX12 ax12_5 (D1, D0, 5, 1000000);*/
00147 
00148     /* Interrupt to start the robot */
00149     startup.fall(&go);
00150     /*motor1->Move(StepperMotor::BWD,6000);
00151     motor2->Move(StepperMotor::FWD,6000);
00152     motor1->WaitWhileActive();
00153     motor2->WaitWhileActive();
00154     motor1->HardStop();
00155     motor2->HardStop();*/
00156     //deplacement(10, 0);
00157     /*    rotation(90, 30);
00158         wait(2);
00159         rotation(45, 30);
00160         wait(2);
00161         rotation(180, 30);
00162         wait(2);
00163         rotation(45, 30);*/
00164     /*motor1->Move(StepperMotor::FWD,6000);
00165     motor2->Move(StepperMotor::BWD,6000);
00166     motor1->WaitWhileActive();
00167     motor2->WaitWhileActive();
00168     motor1->HardStop();
00169     motor2->HardStop();
00170     */
00171 
00172     while(start) {
00173         /* Waiting code */
00174         /*        ax12_1.SetSpeed(70);
00175                 ax12_2.SetSpeed(70);
00176                 ax12_3.SetSpeed(70);
00177                 ax12_4.SetSpeed(70);
00178                 ax12_5.SetSpeed(70);*/
00179         //wait_ms(100);
00180     }
00181 
00182     /* Interrupt to stop the robot */
00183     game_length.attach(&stop, 90); //1 minutes 30 secondes pour la Coupe
00184 
00185     while(end) {
00186 
00187         switch(step) {
00188             case 0:
00189                 step = 1;
00190                 break;
00191 
00192             case 1:
00193                 //Déplacement 1ers cubes au milieu
00194                 printf("deplacement init \n");
00195                 wait_ms(100);
00196                 deplacement(90,1);
00197 
00198                 step = 2;
00199                 break;
00200 
00201             case 2:
00202                 //Mise en place pour pêche
00203                 //motor1->Move(StepperMotor::FWD,4000); //première rotation 90°
00204 //                motor2->Move(StepperMotor::FWD,4000);
00205 //                motor1->WaitWhileActive();
00206 //                motor2->WaitWhileActive();
00207 //                motor1->HardStop();
00208 //                motor2->HardStop();
00209 //                printf("Rotation 1 \n");
00210 //
00211 //
00212 //                motor1->Move(StepperMotor::BWD,6000); //Avance jusqu'au mur
00213 //                motor2->Move(StepperMotor::FWD,6000);
00214 //                motor1->WaitWhileActive();
00215 //                motor2->WaitWhileActive();
00216 //                motor1->HardStop();
00217 //                motor2->HardStop();
00218 //
00219 //                motor1->Move(StepperMotor::FWD,600); //recul pour rotation
00220 //                motor2->Move(StepperMotor::BWD,600);
00221 //                motor1->WaitWhileActive();
00222 //                motor2->WaitWhileActive();
00223 //                motor1->HardStop();
00224 //                motor2->HardStop();
00225 //                printf("Recul Rotation 2\n");
00226 //
00227 //
00228 //
00229 //                motor1->Move(StepperMotor::FWD,4000); //deuxième rotation 90° (même sens que première)
00230 //                motor2->Move(StepperMotor::FWD,4000);
00231 //                motor1->WaitWhileActive();
00232 //                motor2->WaitWhileActive();
00233 //                motor1->HardStop();
00234 //                motor2->HardStop();
00235 //                printf("Rotation 2 \n");
00236 //
00237 //
00238 //                motor1->Move(StepperMotor::BWD,6000); //mise en position
00239 //                motor2->Move(StepperMotor::FWD,6000);
00240 //                motor1->WaitWhileActive();
00241 //                motor2->WaitWhileActive();
00242 //                motor1->HardStop();
00243 //                motor2->HardStop();
00244 //                printf("Mise en position\n");
00245 //
00246 //                motor2->Move(StepperMotor::BWD,4000); //troisième rotation 90°
00247 //                motor1->Move(StepperMotor::BWD,4000);
00248 //                motor1->WaitWhileActive();
00249 //                motor2->WaitWhileActive();
00250 //                motor1->HardStop();
00251 //                motor2->HardStop();
00252 //                printf("Rotation 3 \n");
00253 //
00254 //
00255 //                motor2->Move(StepperMotor::BWD,500); //Collage mur
00256 //                motor1->Move(StepperMotor::FWD,500);
00257 //                motor1->WaitWhileActive();
00258 //                motor2->WaitWhileActive();
00259 //                motor1->HardStop();
00260 //                motor2->HardStop();
00261 //                printf("Collage\n");
00262 //
00263 //
00264 //                motor2->Move(StepperMotor::FWD,600); //recul pour rotation
00265 //                motor1->Move(StepperMotor::BWD,600);
00266 //                motor1->WaitWhileActive();
00267 //                motor2->WaitWhileActive();
00268 //                motor1->HardStop();
00269 //                motor2->HardStop();
00270 //
00271 //                motor2->Move(StepperMotor::BWD,4000); //quatrième rotation 90°
00272 //                motor1->Move(StepperMotor::BWD,4000);
00273 //                motor1->WaitWhileActive();
00274 //                motor2->WaitWhileActive();
00275 //                motor1->HardStop();
00276 //                motor2->HardStop();
00277 //                printf("Rotation 4 \n");
00278                 printf("deplacement arriere 40 \n");
00279                 deplacement(40,0);
00280                 printf("rotation 90 1 \n");
00281                 rotation(90, 30, 0);
00282                 printf("Rotation 1 \n");
00283                 printf("deplacement coller mur\n");
00284                 deplacement(112,1);
00285                 printf("deplacement arriere\n");
00286                 deplacement(3,0);
00287                 printf("rotation finale\n");
00288                 rotation(90, 30, 1);
00289                 printf("avancement 40\n");
00290                 deplacement(40,1);
00291 
00292                 //Mise en position du robot pour pêche
00293 
00294                 step = 3;
00295                 break;
00296             case 3:
00297                 //déploiement des bras, pêche
00298 
00299                 step = 4;
00300                 break;
00301             case 4:
00302                 //Dépose des poissons dans le filet
00303 
00304                 step = 5;
00305 
00306             case 5:
00307                 break;
00308 
00309 
00310         }
00311     }
00312 
00313     parasol.write(0.5);
00314     printf("2");
00315     motor1->HardStop();
00316     motor2->HardStop();
00317 }
00318 
00319 void pwm()
00320 {
00321 }
Generated on Mon Jul 25 2022 08:19:01 by  doxygen 1.7.2