FRC - Hackathon
For Kenya Workshop
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
Diff: main.cpp
- Revision:
- 8:2d4887624f94
- Parent:
- 7:6d58adc26b78
--- a/main.cpp Tue Jun 05 13:55:04 2018 +0000
+++ b/main.cpp Thu Oct 24 13:21:27 2019 +0000
@@ -9,9 +9,7 @@
#define PI 3.1415926535898
-#define NSpeed 100
-
-Serial pc (PA_2, PA_3, 921600);
+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);
@@ -20,157 +18,28 @@
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);
-VMA306 UltraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);
+PIXY pixy (PA_0, PA_1, 115200);
-PIXY Pixy (PA_0, PA_1, 230400);
-
-DigitalIn bp (PC_13);
+InterruptIn bp (PC_13);
DigitalOut led1 (PA_5);
DigitalOut led2 (PD_2);
-DigitalOut disquette (PA_12);
+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;
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);
}
}
\ No newline at end of file