borrowed codes
Dependencies: mbed lib_FRC_2019
main.cpp
- Committer:
- haarkon
- Date:
- 2018-06-05
- Revision:
- 7:6d58adc26b78
- Parent:
- 5:6afed0e241cb
- Child:
- 8:7b0fa8a914c0
File content as of revision 7:6d58adc26b78:
#include "mbed.h" #include "CMPS03.h" #include "CNY70.h" #include "GP2A.h" #include "RC_Servo.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 (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); GP2A ld1 (PA_4, 20, 150, 55); GP2A sd1 (PB_0, 5, 80, 10); RC_Servo ballon (PB_10, 0); RC_Servo verrou (PA_15, 0); VMA306 UltraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2); PIXY Pixy (PA_0, PA_1, 230400); DigitalIn bp (PC_13); DigitalOut led1 (PA_5); DigitalOut led2 (PD_2); DigitalOut disquette (PA_12); main () { int etatmvt = 0; 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\rHelloWorld\n"); led1 = 1; led2 = 0; disquette = 0; //motor.resetPosition(); wait (5); 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()); pc.printf("\rGP2 longue distance = %5.2f\n",ld1.getDistance ()); pc.printf("\rGP2 courte distance = %5.2f\n",sd1.getDistance ()); pc.printf("\rpos = %2.1lf\n",pos); ballon.write (pos); verrou.write (pos); nbiter++; if (nbiter%5==0) { if (sens) { pos += 0.1; if (pos>0.9) sens = 0; } else { pos-=0.1; if (pos<0.1) sens = 1; } } 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 (0.2); } }