borrowed codes
Dependencies: mbed lib_FRC_2019
main.cpp
- Committer:
- kyxstark
- Date:
- 2019-06-07
- Revision:
- 9:234439133426
- Parent:
- 8:7b0fa8a914c0
File content as of revision 9:234439133426:
#include "mbed.h" #include "CMPS03.h" #include "CNY70.h" #include "VMA306.h" #include "Pixy.h" #include "PID.h" #define PI 3.1415926535898 #define NSpeed 100 Serial pc (PA_2, PA_3, 921600); PID motor (TIM3, TIM4, PA_8, PA_9, PC_6, PC_5, PC_9, PC_8); CMPS03 boussole (PC_4); CNY70 ligneD (PC_3); CNY70 ligneG (PC_2); CNY70 exterior (PA_7); VMA306 UltraSon (PB_13, PB_2, PB_14, PC_7, PB_15, PA_6); PIXY Pixy (PA_0, PA_1, 230400); DigitalIn bp (PC_13); DigitalOut led1 (PA_5); DigitalOut led2 (PD_2); main () { led1 = 0; led2 = 0; pc.printf ("\n\rHelloWorld\n"); wait(0.5); led1 = 1; led2 = 0; int etatmvt = 00; int sens = 1, nbiter = 0, nbCC, nbNM; double pos = 0.5; T_pixyCCBloc CCBuf; T_pixyNMBloc NMBuf; double x, y, theta, vG, vD; pc.printf ("\n\rinit\n"); led1 = 1; led2 = 0; motor.resetPosition(); pc.printf ("\n\r3\n"); wait (1); pc.printf ("\n\r2\n"); wait (1); pc.printf ("\n\r1\n"); wait (1); //motor.setSpeed (0,0); while (1) { pc.printf ("\rCap = %5.2lf\n", boussole.getBearing()); if (exterior.whatAmIOn()) pc.printf("\rwhite"); else pc.printf("\rblue "); if (ligneG.whatAmIOn()) pc.printf("\r\twhite"); else pc.printf("\r\tblue "); if (ligneD.whatAmIOn()) pc.printf("\r\t\twhite\n"); else pc.printf("\r\t\tblue\n"); pc.printf ("\r%4.3lf\t %4.3lf\t %4.3lf\n", exterior.getVoltage(), ligneG.getVoltage(), ligneD.getVoltage()); if (UltraSon.isUSGReady()) pc.printf ("\rusG = %5.2lf -", UltraSon.readUSG()); if (UltraSon.isUSBReady()) pc.printf ("\r\t\t usB = %5.2lf -", UltraSon.readUSB()); if (UltraSon.isUSDReady()) pc.printf ("\r\t\t\t\t usD = %5.2lf", UltraSon.readUSD()); pc.printf ("\n"); if (Pixy.checkNewImage()) { Pixy.detectedObject (&nbNM, &nbCC); pc.printf ("\rnbCC = %2d - nbNM = %2d\n", nbCC, nbNM); while (nbCC > 0) { CCBuf = Pixy.getCCBloc (); nbCC--; pc.printf ("\rCC %5d : x=%5d, y=%5d - w=%5d, h=%5d, a=%5d\n", CCBuf.signature, CCBuf.x, CCBuf.y, CCBuf.width, CCBuf.height, (short)CCBuf.angle); } while (nbNM > 0) { NMBuf = Pixy.getNMBloc (); nbNM--; pc.printf ("\rNM %4x : x=%5d, y=%5d - w=%5d, h=%5d\n", NMBuf.signature, NMBuf.x, NMBuf.y, NMBuf.width, NMBuf.height); } } // Square danse !!! motor.getPosition (&x,&y, &theta); motor.getSpeed (&vG, &vD); pc.printf ("\rEtape = %d : x = %5.1lf, y = %5.1lf, theta = %5.1lf - speedG = %5.1lf, speedD = %5.1lf\n", etatmvt, x, y, 180*theta/PI, vG, vD); switch (etatmvt) { case 0 : // On avance de 50cm (coordonnés X = 500, Y = 0) motor.setSpeed (NSpeed,NSpeed); if (x > 300) { etatmvt = 1; } break; case 1 : // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2 motor.setSpeed (NSpeed,-NSpeed); if (theta < (-PI/2.0)) { etatmvt = 2; } break; case 2 : // On avance de 50cm (coordonnés X = 500, Y = -500) motor.setSpeed (NSpeed,NSpeed); if (y < -300) { etatmvt = 3; } break; case 3 : // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage) motor.setSpeed (NSpeed,-NSpeed); if (theta > 0) { etatmvt = 4; } break; case 4 : // On avance de 50cm (coordonnés X = 0, Y = -500) motor.setSpeed (NSpeed,NSpeed); if (x < 0) { etatmvt = 5; } break; case 5 : // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2 motor.setSpeed (NSpeed,-NSpeed); if (theta < (PI/2.0)) { etatmvt = 6; } break; case 6 : // On avance de 50cm (coordonnés X = 0, Y = 0) motor.setSpeed (NSpeed,NSpeed); if (y > 0) { etatmvt = 7; } break; case 7 : // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0 motor.setSpeed (NSpeed,-NSpeed); if (theta < 0) { etatmvt = 0; } break; } pc.printf ("\n\n"); led1 = !led1; led2 = !led2; wait (1); } }