motor.cpp

00001 #include "motor.h"
00002 
00003 
00004 PwmOut moteurA_avancer (PTC2);
00005 PwmOut moteurA_reculer (PTC1);
00006 
00007 PwmOut moteurB_avancer (PTC4);
00008 PwmOut moteurB_reculer (PTC3);
00009 
00010 
00011 PwmOut servo (PTB0);
00012 
00013 void PWM_motor (int vitesse_gauche ,int vitesse_droite)
00014 {  
00015 
00016         if(vitesse_gauche >= 0)//On avance
00017         {
00018             moteurA_reculer.write(0);
00019             moteurA_avancer.write((vitesse_gauche*CORRECTION)/100.0);
00020         }
00021         else//On recule
00022         {
00023             moteurA_avancer.write(0);
00024             moteurA_reculer.write(vitesse_gauche/100.0);
00025         }          
00026 
00027   
00028         if(vitesse_droite >= 0)//On avance
00029         {
00030             moteurB_reculer.write(0);
00031             moteurB_avancer.write(vitesse_droite/100.0);
00032         }
00033         else//On recule
00034         {
00035             moteurB_avancer.write(0);
00036             moteurB_reculer.write(vitesse_droite/100.0);
00037         }
00038  
00039 }
00040 void motor_init()
00041 {
00042     moteurA_avancer.period(1.0/FREQ_PWM);
00043     moteurB_avancer.write(0);
00044     moteurA_avancer.write(0);
00045     moteurB_reculer.write(0);
00046     moteurA_reculer.write(0);
00047     activate_motor = 1;
00048     
00049 }
00050 void angle_servo_moteur (double angle)
00051 {
00052     if(angle > 30) angle = 30;
00053     if(angle < -30) angle = -30;
00054     angle = (angle * PWM_ANGLE_DROIT_MAX) / ANGLE_DROIT_MAX;
00055     angle += 0.0725; 
00056     servo.write(angle);
00057 }