FRC - Hackathon
Programme de test avec librairies minimales et un seul ticker
Revision 0:c3b54b05bf77, committed 2018-06-06
- Comitter:
- haarkon
- Date:
- Wed Jun 06 12:48:02 2018 +0000
- Commit message:
- Programme de test n'utilisant que les biblioth?ques minimales;
Changed in this revision
diff -r 000000000000 -r c3b54b05bf77 PID.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Wed Jun 06 12:48:02 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/FRC-Hackathon/code/PID/#4553677e8b99
diff -r 000000000000 -r c3b54b05bf77 Pixy.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Pixy.lib Wed Jun 06 12:48:02 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/FRC-Hackathon/code/Pixy/#5982d904f7aa
diff -r 000000000000 -r c3b54b05bf77 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Jun 06 12:48:02 2018 +0000
@@ -0,0 +1,253 @@
+#include "mbed.h"
+#include "Pixy.h"
+#include "PID.h"
+
+Serial pc (PA_2, PA_3, 921600);
+
+PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
+
+InterruptIn boussole (PC_4);
+
+AnalogIn CNY70ligneD (PC_3);
+AnalogIn CNY70ligneG (PC_2);
+AnalogIn CNY70exterior (PA_7);
+
+AnalogIn GP2sensorL (PA_4);
+AnalogIn GP2sensorS (PB_0);
+
+DigitalOut trig1 (PB_15);
+InterruptIn echo1 (PA_6);
+DigitalOut trig2 (PB_14);
+InterruptIn echo2 (PC_7);
+DigitalOut trig3 (PB_13);
+InterruptIn echo3 (PB_2);
+
+//PwmOut ballon (PB_10);
+//PwmOut verrou (PA_15);
+
+PIXY pixy (PA_0, PA_1, 230400);
+
+DigitalIn bp (PC_13);
+
+DigitalOut led1 (PA_5);
+DigitalOut led2 (PD_2);
+DigitalOut disquette (PA_12);
+
+Timer temps;
+Ticker tick;
+
+bool CMPS03flag, VMA306Flag1, VMA306Flag2, VMA306Flag3;
+long CMPS03startTime, VMA306startTime1, VMA306startTime2, VMA306startTime3;
+double CMPS03bearing, VMA306dist1, VMA306dist2, VMA306dist3;
+double _Kp, _Ki, _Kd;
+double _SpeedG, _SpeedD;
+double _measDistG, _measDistD;
+int _WheelStuckG, _WheelStuckD;
+double _X, _Y, _THETA;
+
+void VMA306rise1(void)
+{
+ VMA306startTime1 = temps.read_us();
+}
+
+void VMA306fall1(void)
+{
+ VMA306dist1 = (double)(temps.read_us() - VMA306startTime1)/58.0;
+ VMA306Flag1 = 1;
+}
+
+// Clear and start the timer at the begining of the echo pulse
+void VMA306rise2(void)
+{
+ VMA306startTime2 = temps.read_us();
+}
+
+// Stop and read the timer at the end of the pulse
+void VMA306fall2(void)
+{
+ VMA306dist2 = (double)(temps.read_us() - VMA306startTime2)/58.0;
+ VMA306Flag2 = 1;
+}
+
+// Clear and start the timer at the begining of the echo pulse
+void VMA306rise3(void)
+{
+ VMA306startTime3 = temps.read_us();
+}
+
+// Stop and read the timer at the end of the pulse
+void VMA306fall3(void)
+{
+ VMA306dist3 = (double)(temps.read_us() - VMA306startTime3)/58.0;
+ VMA306Flag3 = 1;
+}
+
+void CMPS03rise (void)
+{
+ CMPS03startTime = temps.read_us();
+}
+
+void CMPS03fall (void)
+{
+ CMPS03bearing = ((double)(temps.read_us() - CMPS03startTime - 1000)/100.0);
+}
+
+double getGP2Distance (double sensorValue, double slope)
+{
+ return slope/(sensorValue * 3.3);
+}
+
+bool whatIsUnderMyCNY (double sensorValue)
+{
+ return (sensorValue > 0.5) ? 0 : 1;
+}
+
+void baisser (PwmOut *ptr)
+{
+ ptr->pulsewidth_ms(2);
+}
+
+void lever (PwmOut *ptr)
+{
+ ptr->pulsewidth_ms(1);
+}
+
+
+main ()
+{
+ int nbCC, nbNM, objet = 0;
+ T_pixyCCBloc CCBuf[20];
+ T_pixyNMBloc NMBuf[20];
+ double topDepart, capAuDepart;
+ double x, y, theta;
+ double speedG = 1000, speedD = 1000;
+ PwmOut *ptr;
+
+ boussole.rise (&CMPS03rise);
+ boussole.fall (&CMPS03fall);
+
+ echo1.rise (&VMA306rise1);
+ echo1.fall (&VMA306fall1);
+ echo2.rise (&VMA306rise2);
+ echo2.fall (&VMA306fall2);
+ echo3.rise (&VMA306rise3);
+ echo3.fall (&VMA306fall3);
+
+ temps.start();
+
+/* ptr = new PwmOut(PB_10);
+ ptr->period_ms(20);
+ baisser(ptr);
+ wait (2);
+ delete ptr;
+
+ ptr = new PwmOut(PA_15);
+ ptr->period_ms(20);
+ lever(ptr);
+ wait (2);
+ delete ptr;*/
+
+ pc.printf ("\n\rHelloWorld\n");
+ led1 = 1;
+ led2 = 0;
+ disquette = 0;
+ motor.resetPosition();
+ wait(3);
+
+ pc.printf("\rcheck Pixy\n");
+ if (pixy.checkPixy()) led2=1;
+ else {
+ pc.printf("\rLa pixy ne parle pas\n");
+ while(1);
+ }
+ pc.printf("\rApprochez une balle\n");
+ do {
+ pixy.detectedObject(&nbNM, &nbCC);
+ } while(nbNM!=1);
+ pc.printf("\rOK !\n");
+ led1 = 0;
+
+ pc.printf("\rApprochez une balise\n");
+ do {
+ pixy.detectedObject(&nbNM, &nbCC);
+ } while(nbCC!=1);
+ pc.printf("\rOK !\n");
+ led2 = 0;
+
+ pc.printf("\n\rcheck Moteur\n");
+ while(bp.read());
+ led1 = 1;
+
+ //Avance
+ motor.setSpeed (80,80);
+ do {
+ motor.getPosition (&x,&y, &theta);
+ } while (x<50);
+ motor.setSpeed (0,0);
+ wait_ms(10);
+
+ //Recule
+ motor.setSpeed (-80,-80);
+ do {
+ motor.getPosition (&x,&y, &theta);
+ } while (x>5);
+ motor.setSpeed (0,0);
+ wait_ms(10);
+
+ //Tourne à droite
+ motor.setSpeed (80,-80);
+ do {
+ motor.getPosition (&x,&y, &theta);
+ } while (theta > -PI/2.0);
+
+ motor.setSpeed (0,0);
+ pc.printf("\rMoteurs OK\n");
+ while (bp.read());
+
+ pc.printf("\rScan de la zone\n");
+
+ motor.setSpeed (-50,50);
+ do {
+ pixy.detectedObject(&nbNM, &nbCC);
+ } while (nbNM != 1);
+
+ pc.printf("\rBalle trouvee\n");
+ pc.printf("\rAlignement\n");
+
+ motor.setSpeed (-30,30);
+ do {
+ pixy.detectedObject(&nbNM, &nbCC);
+ if (nbNM==1) {
+ NMBuf[objet] = pixy.getNMBloc ();
+ nbNM--;
+ }
+ } while (NMBuf[objet].x != 160);
+
+ pc.printf("\rBalle alignee\n");
+ pc.printf("\n\rREADY TO GO\n");
+
+ do {
+ led1 = !led1;
+ wait(0.1);
+ } while (bp);
+
+ topDepart = temps.read();
+ capAuDepart = CMPS03bearing;
+
+ while (1) {
+ do {
+ motor.setSpeed (speedG, speedD);
+ pixy.detectedObject(&nbNM, &nbCC);
+ if (nbNM == 1) {
+ NMBuf[0] = pixy.getNMBloc ();
+ nbNM--;
+ }
+ speedG = 1000 - 10*(double)(NMBuf[0].x - 160);
+ speedD = 1000 - 10*(double)(NMBuf[0].x + 160);
+ } while (NMBuf[0].y < 170);
+
+
+
+
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r c3b54b05bf77 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Jun 06 12:48:02 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/5aab5a7997ee \ No newline at end of file