Test code 29-10-2019
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
main.cpp@5:6afed0e241cb, 2018-06-05 (annotated)
- Committer:
- haarkon
- Date:
- Tue Jun 05 08:02:56 2018 +0000
- Revision:
- 5:6afed0e241cb
- Parent:
- 4:5038b4cd1088
- Child:
- 7:6d58adc26b78
untested;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
haarkon | 0:4902b3f17137 | 1 | #include "mbed.h" |
haarkon | 0:4902b3f17137 | 2 | #include "CMPS03.h" |
haarkon | 0:4902b3f17137 | 3 | #include "CNY70.h" |
haarkon | 0:4902b3f17137 | 4 | #include "GP2A.h" |
haarkon | 0:4902b3f17137 | 5 | #include "RC_Servo.h" |
haarkon | 0:4902b3f17137 | 6 | #include "VMA306.h" |
haarkon | 3:0221daeeaa86 | 7 | #include "Pixy.h" |
haarkon | 4:5038b4cd1088 | 8 | #include "PID.h" |
haarkon | 4:5038b4cd1088 | 9 | |
haarkon | 4:5038b4cd1088 | 10 | #define PI 3.1415926535898 |
haarkon | 0:4902b3f17137 | 11 | |
haarkon | 3:0221daeeaa86 | 12 | Serial pc (PA_2, PA_3, 921600); |
haarkon | 5:6afed0e241cb | 13 | //PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5); |
haarkon | 0:4902b3f17137 | 14 | |
haarkon | 3:0221daeeaa86 | 15 | CMPS03 boussole (PC_4); |
haarkon | 0:4902b3f17137 | 16 | |
haarkon | 3:0221daeeaa86 | 17 | CNY70 ligneD (PC_3); |
haarkon | 3:0221daeeaa86 | 18 | CNY70 ligneG (PC_2); |
haarkon | 3:0221daeeaa86 | 19 | CNY70 exterior (PA_7); |
haarkon | 0:4902b3f17137 | 20 | |
haarkon | 3:0221daeeaa86 | 21 | GP2A ld1 (PA_4, 20, 150, 55); |
haarkon | 3:0221daeeaa86 | 22 | GP2A sd1 (PB_0, 5, 80, 10); |
haarkon | 0:4902b3f17137 | 23 | |
haarkon | 4:5038b4cd1088 | 24 | RC_Servo ballon (PA_10, 0); |
haarkon | 4:5038b4cd1088 | 25 | RC_Servo verrou (PA_11, 0); |
haarkon | 0:4902b3f17137 | 26 | |
haarkon | 3:0221daeeaa86 | 27 | VMA306 UltraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2); |
haarkon | 0:4902b3f17137 | 28 | |
haarkon | 3:0221daeeaa86 | 29 | PIXY Pixy (PA_0, PA_1, 230400); |
haarkon | 0:4902b3f17137 | 30 | |
haarkon | 4:5038b4cd1088 | 31 | |
haarkon | 0:4902b3f17137 | 32 | DigitalOut led1 (PA_5); |
haarkon | 0:4902b3f17137 | 33 | DigitalOut led2 (PD_2); |
haarkon | 0:4902b3f17137 | 34 | DigitalOut disquette (PA_12); |
haarkon | 0:4902b3f17137 | 35 | |
haarkon | 3:0221daeeaa86 | 36 | main () |
haarkon | 3:0221daeeaa86 | 37 | { |
haarkon | 4:5038b4cd1088 | 38 | int etatmvt = 0; |
haarkon | 3:0221daeeaa86 | 39 | int sens = 1, nbiter = 0, nbCC, nbNM; |
haarkon | 3:0221daeeaa86 | 40 | double pos = 0.5; |
haarkon | 3:0221daeeaa86 | 41 | T_pixyCCBloc CCBuf; |
haarkon | 3:0221daeeaa86 | 42 | T_pixyNMBloc NMBuf; |
haarkon | 4:5038b4cd1088 | 43 | double x, y, theta; |
haarkon | 3:0221daeeaa86 | 44 | |
haarkon | 3:0221daeeaa86 | 45 | pc.printf ("\n\rHelloWorld\n"); |
haarkon | 3:0221daeeaa86 | 46 | led1 = 1; |
haarkon | 3:0221daeeaa86 | 47 | led2 = 0; |
haarkon | 3:0221daeeaa86 | 48 | disquette = 0; |
haarkon | 4:5038b4cd1088 | 49 | |
haarkon | 5:6afed0e241cb | 50 | //motor.resetPosition(); |
haarkon | 3:0221daeeaa86 | 51 | |
haarkon | 3:0221daeeaa86 | 52 | wait (5); |
haarkon | 3:0221daeeaa86 | 53 | |
haarkon | 3:0221daeeaa86 | 54 | while (1) { |
haarkon | 3:0221daeeaa86 | 55 | pc.printf ("\rCap = %5.2lf\n", boussole.getBearing()); |
haarkon | 3:0221daeeaa86 | 56 | |
haarkon | 3:0221daeeaa86 | 57 | if (exterior.whatAmIOn()) pc.printf("\rwhite"); |
haarkon | 3:0221daeeaa86 | 58 | else pc.printf("\rblue "); |
haarkon | 3:0221daeeaa86 | 59 | if (ligneG.whatAmIOn()) pc.printf("\r\twhite"); |
haarkon | 3:0221daeeaa86 | 60 | else pc.printf("\r\tblue "); |
haarkon | 3:0221daeeaa86 | 61 | if (ligneD.whatAmIOn()) pc.printf("\r\t\twhite\n"); |
haarkon | 3:0221daeeaa86 | 62 | else pc.printf("\r\t\tblue\n"); |
haarkon | 3:0221daeeaa86 | 63 | |
haarkon | 3:0221daeeaa86 | 64 | pc.printf ("\r%4.3lf\t %4.3lf\t %4.3lf\n", exterior.getVoltage(), ligneG.getVoltage(), ligneD.getVoltage()); |
haarkon | 3:0221daeeaa86 | 65 | |
haarkon | 3:0221daeeaa86 | 66 | pc.printf("\rGP2 longue distance = %5.2f\n",ld1.getDistance ()); |
haarkon | 3:0221daeeaa86 | 67 | pc.printf("\rGP2 courte distance = %5.2f\n",sd1.getDistance ()); |
haarkon | 2:e1372e7b671f | 68 | |
haarkon | 3:0221daeeaa86 | 69 | pc.printf("\rpos = %2.1lf\n",pos); |
haarkon | 5:6afed0e241cb | 70 | ballon.write (pos); |
haarkon | 3:0221daeeaa86 | 71 | verrou.write (pos); |
haarkon | 3:0221daeeaa86 | 72 | nbiter++; |
haarkon | 3:0221daeeaa86 | 73 | if (nbiter%5==0) { |
haarkon | 3:0221daeeaa86 | 74 | if (sens) { |
haarkon | 3:0221daeeaa86 | 75 | pos += 0.1; |
haarkon | 3:0221daeeaa86 | 76 | if (pos>0.9) sens = 0; |
haarkon | 3:0221daeeaa86 | 77 | } else { |
haarkon | 3:0221daeeaa86 | 78 | pos-=0.1; |
haarkon | 3:0221daeeaa86 | 79 | if (pos<0.1) sens = 1; |
haarkon | 3:0221daeeaa86 | 80 | } |
haarkon | 5:6afed0e241cb | 81 | } |
haarkon | 3:0221daeeaa86 | 82 | |
haarkon | 3:0221daeeaa86 | 83 | if (UltraSon.isUSGReady()) pc.printf ("\rusG = %5.2lf -", UltraSon.readUSG()); |
haarkon | 3:0221daeeaa86 | 84 | if (UltraSon.isUSBReady()) pc.printf ("\r\t\t usB = %5.2lf -", UltraSon.readUSB()); |
haarkon | 3:0221daeeaa86 | 85 | if (UltraSon.isUSDReady()) pc.printf ("\r\t\t\t\t usD = %5.2lf", UltraSon.readUSD()); |
haarkon | 3:0221daeeaa86 | 86 | pc.printf ("\n"); |
haarkon | 2:e1372e7b671f | 87 | |
haarkon | 3:0221daeeaa86 | 88 | if (Pixy.checkNewImage()) { |
haarkon | 3:0221daeeaa86 | 89 | Pixy.detectedObject (&nbNM, &nbCC); |
haarkon | 3:0221daeeaa86 | 90 | pc.printf ("\rnbCC = %2d - nbNM = %2d\n", nbCC, nbNM); |
haarkon | 3:0221daeeaa86 | 91 | while (nbCC > 0) { |
haarkon | 3:0221daeeaa86 | 92 | CCBuf = Pixy.getCCBloc (); |
haarkon | 3:0221daeeaa86 | 93 | nbCC--; |
haarkon | 3:0221daeeaa86 | 94 | 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); |
haarkon | 3:0221daeeaa86 | 95 | } |
haarkon | 3:0221daeeaa86 | 96 | while (nbNM > 0) { |
haarkon | 3:0221daeeaa86 | 97 | NMBuf = Pixy.getNMBloc (); |
haarkon | 3:0221daeeaa86 | 98 | nbNM--; |
haarkon | 3:0221daeeaa86 | 99 | pc.printf ("\rNM %4x : x=%5d, y=%5d - w=%5d, h=%5d\n", NMBuf.signature, NMBuf.x, NMBuf.y, NMBuf.width, NMBuf.height); |
haarkon | 3:0221daeeaa86 | 100 | } |
haarkon | 3:0221daeeaa86 | 101 | } |
haarkon | 3:0221daeeaa86 | 102 | |
haarkon | 4:5038b4cd1088 | 103 | // Square danse !!! |
haarkon | 5:6afed0e241cb | 104 | /*motor.getPosition (&x,&y, &theta); |
haarkon | 4:5038b4cd1088 | 105 | switch (etatmvt) { |
haarkon | 4:5038b4cd1088 | 106 | case 0 : |
haarkon | 4:5038b4cd1088 | 107 | // On avance de 50cm (coordonnés X = 500, Y = 0) |
haarkon | 4:5038b4cd1088 | 108 | motor.setSpeed (400,400); |
haarkon | 4:5038b4cd1088 | 109 | if (x >= 500) etatmvt = 1; |
haarkon | 4:5038b4cd1088 | 110 | break; |
haarkon | 4:5038b4cd1088 | 111 | case 1 : |
haarkon | 4:5038b4cd1088 | 112 | // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2 |
haarkon | 4:5038b4cd1088 | 113 | motor.setSpeed (400,-400); |
haarkon | 4:5038b4cd1088 | 114 | if (theta <= -PI/2.0) etatmvt = 2; |
haarkon | 4:5038b4cd1088 | 115 | break; |
haarkon | 4:5038b4cd1088 | 116 | case 2 : |
haarkon | 4:5038b4cd1088 | 117 | // On avance de 50cm (coordonnés X = 500, Y = -500) |
haarkon | 4:5038b4cd1088 | 118 | motor.setSpeed (400,400); |
haarkon | 4:5038b4cd1088 | 119 | if (y <= -500) etatmvt = 3; |
haarkon | 4:5038b4cd1088 | 120 | break; |
haarkon | 4:5038b4cd1088 | 121 | case 3 : |
haarkon | 4:5038b4cd1088 | 122 | // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage) |
haarkon | 4:5038b4cd1088 | 123 | motor.setSpeed (400,-400); |
haarkon | 4:5038b4cd1088 | 124 | if (theta >= 0) etatmvt = 4; |
haarkon | 4:5038b4cd1088 | 125 | break; |
haarkon | 4:5038b4cd1088 | 126 | case 4 : |
haarkon | 4:5038b4cd1088 | 127 | // On avance de 50cm (coordonnés X = 0, Y = -500) |
haarkon | 4:5038b4cd1088 | 128 | motor.setSpeed (400,400); |
haarkon | 4:5038b4cd1088 | 129 | if (x <= 0) etatmvt = 5; |
haarkon | 4:5038b4cd1088 | 130 | break; |
haarkon | 4:5038b4cd1088 | 131 | case 5 : |
haarkon | 4:5038b4cd1088 | 132 | // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2 |
haarkon | 4:5038b4cd1088 | 133 | motor.setSpeed (400,-400); |
haarkon | 4:5038b4cd1088 | 134 | if (theta <= PI/2.0) etatmvt = 6; |
haarkon | 4:5038b4cd1088 | 135 | break; |
haarkon | 4:5038b4cd1088 | 136 | case 6 : |
haarkon | 4:5038b4cd1088 | 137 | // On avance de 50cm (coordonnés X = 0, Y = 0) |
haarkon | 4:5038b4cd1088 | 138 | motor.setSpeed (400,400); |
haarkon | 4:5038b4cd1088 | 139 | if (y <= 0) etatmvt = 7; |
haarkon | 4:5038b4cd1088 | 140 | break; |
haarkon | 4:5038b4cd1088 | 141 | case 7 : |
haarkon | 4:5038b4cd1088 | 142 | // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0 |
haarkon | 4:5038b4cd1088 | 143 | motor.setSpeed (400,-400); |
haarkon | 4:5038b4cd1088 | 144 | if (theta <= 0) etatmvt = 0; |
haarkon | 4:5038b4cd1088 | 145 | break; |
haarkon | 4:5038b4cd1088 | 146 | } |
haarkon | 5:6afed0e241cb | 147 | */ |
haarkon | 3:0221daeeaa86 | 148 | pc.printf ("\n"); |
haarkon | 3:0221daeeaa86 | 149 | led1 = !led1; |
haarkon | 3:0221daeeaa86 | 150 | led2 = !led2; |
haarkon | 3:0221daeeaa86 | 151 | wait (0.2); |
haarkon | 3:0221daeeaa86 | 152 | } |
haarkon | 0:4902b3f17137 | 153 | } |