Commit 25c7cc5bcdebb78eafac1f0c3cd2ea6dead8660b

Authored by Mestari Yousra
1 parent 8167aac3

ajout de avancer, pivoter et etat PLS

Showing 1 changed file with 29 additions and 2 deletions   Show diff stats
... ... @@ -16,6 +16,8 @@
16 16 #include "AX12.h"
17 17 #include "AXS1.h"
18 18  
  19 +#include <math.h>
  20 +
19 21 /* Private typedef -----------------------------------------------------------*/
20 22 /* Private define ------------------------------------------------------------*/
21 23 #define P_GOAL_POSITION_L 30
... ... @@ -261,6 +263,21 @@ void getAngle(unsigned char id, unsigned int* outAngle) {
261 263 }
262 264 }
263 265  
  266 +void avancer(int vitesse){
  267 + setSpeed(MOTOR_up_left, vitesse);
  268 + setSpeed(MOTOR_up_right, vitesse);
  269 + setSpeed(MOTOR_down_left, vitesse);
  270 + setSpeed(MOTOR_down_right, vitesse);
  271 +}
  272 +void pivoter (int rotation){
  273 + setSpeed(MOTOR_up_left, rotation);
  274 + setSpeed(MOTOR_up_right, - rotation);
  275 + setSpeed(MOTOR_down_left, 0);
  276 + setSpeed(MOTOR_down_right, 0);
  277 +}
  278 +
  279 +
  280 +
264 281 /////////////////////////////////////////////////////
265 282 ////////////////////////////// AX S1 ////////////////
266 283 /////////////////////////////////////////////////////
... ... @@ -610,8 +627,18 @@ int main(void)
610 627  
611 628  
612 629 }
613   -
614   - }
  630 +
  631 + while (state==PLS){
  632 + int vitesse=500
  633 + while (field<thresholdInfrared){
  634 + setSpeed(MOTOR_up_left, 1023);
  635 + setSpeed(MOTOR_up_right, vitesse);
  636 + setSpeed(MOTOR_down_left, 1023);
  637 + setSpeed(MOTOR_down_right, vitesse);
  638 + mdelay(floor(M_PI/vitesse));
  639 + vitesse=vitesse+50;
  640 + }
  641 + state==START
615 642  
616 643 while (1) {} ;
617 644  
... ...