![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Test code 29-10-2019
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
main.cpp
- Committer:
- ephymugo
- Date:
- 2019-10-31
- Revision:
- 15:a0a88aeb4fe1
- Parent:
- 14:899c6d29c0a8
- Child:
- 16:08bd2d5d21fb
File content as of revision 15:a0a88aeb4fe1:
#include "mbed.h" #include "CMPS03.h" #include "CNY70.h" #include "GP2A.h" #include "VMA306.h" #include "Pixy.h" #include "PID.h" #define PI 3.1415926535898 Serial pc (PA_2, PA_3, 115200); PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5); CMPS03 boussole (PC_4); CNY70 ligneD (PC_3); CNY70 ligneG (PC_2); CNY70 exterior (PA_7); VMA306 ultraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2); PIXY pixy (PA_0, PA_1, 115200); InterruptIn button (PC_13); DigitalOut led1 (PA_5); DigitalOut led2 (PD_2); DigitalOut unused1 (PB_10); DigitalOut unused2 (PA_15); DigitalOut unused3 (PA_12); DigitalIn unused4 (PA_4, PullUp); DigitalIn unused5 (PB_0, PullUp); DigitalIn unused6 (PC_1, PullUp); DigitalIn unused7 (PC_0, PullUp); Timer temps; typedef enum {IDLE,FORWARD,REVERSE} T_state; main () { double x,y,theta, VL=0, VR=0; T_state state= IDLE; motor.resetPosition(); while (1) { pc.printf("\r x is %lf ",x); motor.getPosition(&x,&y,&theta); motor.setSpeed (VL,VR); switch (state) { case IDLE: if (button==0)state= FORWARD; VL=0; VR=0; break; case FORWARD: if (x<-300) state= REVERSE; VL=-100; VR=-100; break; case REVERSE: if (x>0) state= IDLE; VL=100; VR=100; break; }// switch }// while 1 }// main