issou

Dependencies:   mbed hcsr04

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "hcsr04.h"
00003  
00004 #define     MOT_A_IN_1      D3
00005 #define     MOT_A_IN_2      D4
00006 #define     MOT_A_PWM       D5
00007  
00008 #define     MOT_B_IN_1      D9
00009 #define     MOT_B_IN_2      D7
00010 #define     MOT_B_PWM       D6
00011  
00012 #define     STBY            D2
00013  
00014 #define     ECHO            D11
00015 #define     TRIGGER         D12
00016  
00017 #define     LINE_LEFT       D14
00018 #define     LINE_RIGHT      D15
00019 
00020 #define EPSILON 50
00021 
00022 PwmOut speedA(MOT_A_PWM);
00023 PwmOut speedB(MOT_B_PWM);
00024 
00025 //DigitalOut myled(LED1);
00026 HCSR04 sonar(TRIGGER, ECHO);
00027 Serial pc(USBTX, USBRX);
00028 
00029 DigitalOut inA1(MOT_A_IN_1);
00030 DigitalOut inA2(MOT_A_IN_2);
00031 DigitalOut inB1(MOT_B_IN_1);
00032 DigitalOut inB2(MOT_B_IN_2);
00033 
00034 DigitalOut stby(STBY);
00035 
00036 DigitalIn lineL(LINE_LEFT); // ?+
00037 DigitalIn lineR(LINE_RIGHT); // ?
00038 
00039 // ajout du 
00040  
00041 void testMotors();
00042 void testLineSensorsCustom();
00043 void DetectThing();
00044  
00045 int main() {
00046     //pc.baud(9600);
00047     //pc.printf("Hello World");
00048     //testMotors();
00049     //testLineSensorsCustom();
00050     //DetectThing();
00051     
00052     // Choix de la direction des moteurs, le robot avance.
00053     /*
00054     inA1 = 0;
00055     inA2 = 1;
00056     inB1 = 1;
00057     inB2 = 0;*/
00058     //DetectThing();
00059    
00060     // Choix de la vitesse de rotation des moteurs.
00061     //speedA = 0.3; // Moteur gauche
00062     //speedB = 0.3; // Moteur droit
00063     // vpourcentage par rapport à la vitesse max
00064    testMotors();
00065     // On active les moteurs
00066     //stby = 1;
00067     
00068 }
00069 
00070 void DetectThing()
00071 {
00072     double dist;
00073     
00074     while (1)
00075     {
00076         sonar.start();
00077         dist = sonar.get_dist_cm();
00078         
00079         pc.printf("distance : %f\r\n", dist);
00080         //wait(0.2);
00081     }
00082  
00083 }
00084  
00085 void testMotors()
00086 {
00087     // Choix de la direction des moteurs, le robot avance.
00088     inA1 = 1;
00089     inA2 = 0;
00090     inB1 = 1;
00091     inB2 = 0;
00092    
00093     // Choix de la vitesse de rotation des moteurs.
00094     speedA = 0.01; // Moteur gauche
00095     speedB = 0.01; // Moteur droit
00096     // vpourcentage par rapport à la vitesse max
00097    
00098     // On active les moteurs
00099     stby = 1;
00100     // si stby à 0 ? alors moteur en standby, enfaite stby est inv stby
00101    
00102     wait(0.5); // On avance pendant 1 secondes
00103    
00104     // On tourne pendant 1sec sur la gauche
00105     speedA = 0.5;
00106     wait(0.5);
00107    
00108     // On tourne pendant 1sec sur la droite
00109     speedA = 1;
00110     speedB = 0.5;
00111     wait(0.5);
00112    
00113     // On tourne sur place sur la gauche pendant 1sec
00114     speedB = 1;
00115     inA1 = 0;
00116     inA2 = 1;
00117     wait(0.5);
00118    
00119     // On tourne sur place sur la droite pendant 1sec
00120     inA1 = 1;
00121     inA2 = 0;
00122     inB1 = 0;
00123     inB2 = 1;
00124     wait(0.5);
00125    
00126     // On recule pendant 1sec;
00127     inA1 = 0;
00128     inA2 = 1;
00129     wait(0.5);
00130    
00131     // On avance de nouveau 1sec;
00132     inA1 = 1;
00133     inA2 = 0;
00134     inB1 = 1;
00135     inB2 = 0;
00136     wait(0.5);
00137    
00138     speedA=0;
00139     speedB=0;
00140 }
00141  /*
00142 void testLineSensorsCustom()
00143 {
00144     float dist;
00145     
00146     inA1 = 1;
00147     inA2 = 0;
00148     inB1 = 1;
00149     inB2 = 0;
00150    
00151     stby = 1;
00152    
00153     speedA = 0;
00154     speedB = 0;
00155    
00156     while(dist > EPSILON/2)
00157     {
00158         sonar.start();
00159         dist = sonar.get_dist_cm();
00160         
00161         //pc.printf("distance : %f\r\n", dist);
00162 
00163         speedA = 1;
00164         speedB = .9;
00165         inA1 = 1;
00166         inA2 = 0;
00167         inB1 = 1;
00168         inB2 = 0;
00169     }
00170 
00171     
00172     while(dist < EPSILON/2)
00173     {
00174         sonar.start();
00175         dist = sonar.get_dist_cm();
00176         
00177         speedA = 0;
00178         speedB = 0;
00179         inA1 = inA2 = inB1 = inB2 = 1;
00180         
00181     }
00182 }
00183 
00184 
00185 
00186     -> Choix direction
00187     -> Choix vitesse rotation moteur
00188     
00189 
00190 
00191         if (lineL <= .5)
00192             speedA = 0.9;
00193         else
00194             speedA = 0;
00195         if (lineR <= .5) // ??
00196             speedB = .7;
00197         else
00198             speedB = 0;
00199 */