main.cpp

00001 #include "mbed.h"
00002 //CNY70
00003 DigitalIn CNY_1 (PA_12);
00004 DigitalIn CNY_2 (PB_2);
00005 DigitalIn CNY_3 (PC_6);
00006 DigitalIn CNY_4 (PB_12);
00007 
00008 // Moteur 1
00009 DigitalOut INA1(PA_10);
00010 DigitalOut INB1(PB_3);
00011 DigitalOut EN1(PB_10);
00012 PwmOut PWM1(PB_14);
00013  
00014 // Moteur 2
00015 DigitalOut INA2(PA_8);
00016 DigitalOut INB2(PA_9);
00017 DigitalOut EN2(PA_6);
00018 PwmOut PWM2(PB_13);
00019 
00020 DigitalIn Jack (PB_15, PullUp);
00021 
00022 void init_moteur() ;
00023 
00024  
00025 #define vitesse 0.2
00026  
00027 bool flag=0;
00028 
00029 
00030 
00031 void init_moteur() {
00032     
00033    EN1=1;
00034    INA1 = 0;
00035    INB1 = 0;
00036    PWM1.period(0.00005);   // 20 kHz (valid 0 - 20 kHz)
00037    PWM1.write(0);
00038    EN2=1;
00039    INA2 = 0;
00040    INB2 = 0;
00041    PWM2.period(0.00005);   // 20 kHz (valid 0 - 20 kHz)
00042    PWM2.write(0);
00043    }
00044 
00045 int main() 
00046 {
00047  
00048 
00049 
00050 init_moteur();
00051       while (true) 
00052       {
00053         if (Jack==1){
00054             if (CNY_1==1 and CNY_2==1 and CNY_3==1 and CNY_4==1 and flag==0){
00055                 INA1 = 1;
00056                 INB1 = 0;
00057                INA2 = 0;
00058                INB2 = 1;
00059                PWM1.write(vitesse);
00060                PWM2.write(vitesse);
00061                flag=1;   
00062                wait (0.3);
00063                PWM1.write(0);
00064                PWM2.write(0);
00065             } 
00066             if (CNY_1==0 and CNY_2==0 and CNY_3==0 and CNY_4==0 and flag==0){
00067                 INA1 = 0;
00068                 INB1 = 1;
00069                 INA2 = 1;
00070                 INB2 = 0;
00071                 PWM1.write(vitesse);
00072                 PWM2.write(vitesse+0.005);
00073             }
00074         }
00075         
00076         
00077       }
00078   
00079 }
00080  
00081  
00082