FRC_2019_test - programme test hackathon 2019

FRC - Hackathon » Code » FRC_2019_test

FRC - Hackathon / Mbed 2 deprecated FRC_2019_test

programme test hackathon 2019

Dependencies: mbed lib_FRC_2019

main.cpp@9:234439133426, 2019-06-07 (annotated)

Committer:: kyxstark
Date:: Fri Jun 07 06:06:52 2019 +0000
Revision:: 9:234439133426
Parent:: 8:7b0fa8a914c0

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 "VMA306.h"
haarkon	3:0221daeeaa86	5	#include "Pixy.h"
haarkon	4:5038b4cd1088	6	#include "PID.h"
haarkon	4:5038b4cd1088	7
kyxstark	8:7b0fa8a914c0	8
haarkon	4:5038b4cd1088	9	#define PI 3.1415926535898
haarkon	0:4902b3f17137	10
haarkon	7:6d58adc26b78	11	#define NSpeed 100
haarkon	7:6d58adc26b78	12
haarkon	3:0221daeeaa86	13	Serial pc (PA_2, PA_3, 921600);
kyxstark	8:7b0fa8a914c0	14	PID motor (TIM3, TIM4, PA_8, PA_9, PC_6, PC_5, PC_9, PC_8);
haarkon	0:4902b3f17137	15
haarkon	3:0221daeeaa86	16	CMPS03 boussole (PC_4);
haarkon	0:4902b3f17137	17
haarkon	3:0221daeeaa86	18	CNY70 ligneD (PC_3);
haarkon	3:0221daeeaa86	19	CNY70 ligneG (PC_2);
haarkon	3:0221daeeaa86	20	CNY70 exterior (PA_7);
haarkon	0:4902b3f17137	21
haarkon	0:4902b3f17137	22
kyxstark	8:7b0fa8a914c0	23	VMA306 UltraSon (PB_13, PB_2, PB_14, PC_7, PB_15, PA_6);
haarkon	0:4902b3f17137	24
haarkon	3:0221daeeaa86	25	PIXY Pixy (PA_0, PA_1, 230400);
haarkon	0:4902b3f17137	26
haarkon	7:6d58adc26b78	27	DigitalIn bp (PC_13);
haarkon	4:5038b4cd1088	28
haarkon	0:4902b3f17137	29	DigitalOut led1 (PA_5);
haarkon	0:4902b3f17137	30	DigitalOut led2 (PD_2);
haarkon	0:4902b3f17137	31
haarkon	3:0221daeeaa86	32	main ()
haarkon	3:0221daeeaa86	33	{
kyxstark	8:7b0fa8a914c0	34	led1 = 0;
kyxstark	8:7b0fa8a914c0	35	led2 = 0;
kyxstark	8:7b0fa8a914c0	36	pc.printf ("\n\rHelloWorld\n");
kyxstark	8:7b0fa8a914c0	37	wait(0.5);
kyxstark	8:7b0fa8a914c0	38	led1 = 1;
kyxstark	8:7b0fa8a914c0	39	led2 = 0;
kyxstark	8:7b0fa8a914c0	40
kyxstark	8:7b0fa8a914c0	41	int etatmvt = 00;
haarkon	3:0221daeeaa86	42	int sens = 1, nbiter = 0, nbCC, nbNM;
haarkon	3:0221daeeaa86	43	double pos = 0.5;
haarkon	3:0221daeeaa86	44	T_pixyCCBloc CCBuf;
haarkon	3:0221daeeaa86	45	T_pixyNMBloc NMBuf;
haarkon	7:6d58adc26b78	46	double x, y, theta, vG, vD;
haarkon	3:0221daeeaa86	47
kyxstark	8:7b0fa8a914c0	48	pc.printf ("\n\rinit\n");
haarkon	3:0221daeeaa86	49	led1 = 1;
haarkon	3:0221daeeaa86	50	led2 = 0;
haarkon	4:5038b4cd1088	51
kyxstark	8:7b0fa8a914c0	52	motor.resetPosition();
haarkon	3:0221daeeaa86	53
kyxstark	8:7b0fa8a914c0	54	pc.printf ("\n\r3\n");
kyxstark	8:7b0fa8a914c0	55	wait (1);
kyxstark	8:7b0fa8a914c0	56	pc.printf ("\n\r2\n");
kyxstark	8:7b0fa8a914c0	57	wait (1);
kyxstark	8:7b0fa8a914c0	58	pc.printf ("\n\r1\n");
kyxstark	8:7b0fa8a914c0	59	wait (1);
kyxstark	8:7b0fa8a914c0	60
kyxstark	9:234439133426	61
kyxstark	9:234439133426	62	//motor.setSpeed (0,0);
haarkon	3:0221daeeaa86	63
haarkon	3:0221daeeaa86	64	while (1) {
haarkon	3:0221daeeaa86	65	pc.printf ("\rCap = %5.2lf\n", boussole.getBearing());
haarkon	3:0221daeeaa86	66
haarkon	3:0221daeeaa86	67	if (exterior.whatAmIOn()) pc.printf("\rwhite");
haarkon	3:0221daeeaa86	68	else pc.printf("\rblue ");
haarkon	3:0221daeeaa86	69	if (ligneG.whatAmIOn()) pc.printf("\r\twhite");
haarkon	3:0221daeeaa86	70	else pc.printf("\r\tblue ");
haarkon	3:0221daeeaa86	71	if (ligneD.whatAmIOn()) pc.printf("\r\t\twhite\n");
haarkon	3:0221daeeaa86	72	else pc.printf("\r\t\tblue\n");
haarkon	3:0221daeeaa86	73
haarkon	3:0221daeeaa86	74	pc.printf ("\r%4.3lf\t %4.3lf\t %4.3lf\n", exterior.getVoltage(), ligneG.getVoltage(), ligneD.getVoltage());
haarkon	3:0221daeeaa86	75
haarkon	3:0221daeeaa86	76
haarkon	3:0221daeeaa86	77	if (UltraSon.isUSGReady()) pc.printf ("\rusG = %5.2lf -", UltraSon.readUSG());
haarkon	3:0221daeeaa86	78	if (UltraSon.isUSBReady()) pc.printf ("\r\t\t usB = %5.2lf -", UltraSon.readUSB());
haarkon	3:0221daeeaa86	79	if (UltraSon.isUSDReady()) pc.printf ("\r\t\t\t\t usD = %5.2lf", UltraSon.readUSD());
haarkon	3:0221daeeaa86	80	pc.printf ("\n");
haarkon	2:e1372e7b671f	81
haarkon	3:0221daeeaa86	82	if (Pixy.checkNewImage()) {
haarkon	3:0221daeeaa86	83	Pixy.detectedObject (&nbNM, &nbCC);
haarkon	3:0221daeeaa86	84	pc.printf ("\rnbCC = %2d - nbNM = %2d\n", nbCC, nbNM);
haarkon	3:0221daeeaa86	85	while (nbCC > 0) {
haarkon	3:0221daeeaa86	86	CCBuf = Pixy.getCCBloc ();
haarkon	3:0221daeeaa86	87	nbCC--;
haarkon	3:0221daeeaa86	88	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	89	}
haarkon	3:0221daeeaa86	90	while (nbNM > 0) {
haarkon	3:0221daeeaa86	91	NMBuf = Pixy.getNMBloc ();
haarkon	3:0221daeeaa86	92	nbNM--;
haarkon	3:0221daeeaa86	93	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	94	}
haarkon	3:0221daeeaa86	95	}
haarkon	3:0221daeeaa86	96
haarkon	4:5038b4cd1088	97	// Square danse !!!
haarkon	7:6d58adc26b78	98	motor.getPosition (&x,&y, &theta);
haarkon	7:6d58adc26b78	99	motor.getSpeed (&vG, &vD);
haarkon	7:6d58adc26b78	100
haarkon	7:6d58adc26b78	101	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);
kyxstark	9:234439133426	102
haarkon	4:5038b4cd1088	103	switch (etatmvt) {
haarkon	4:5038b4cd1088	104	case 0 :
haarkon	4:5038b4cd1088	105	// On avance de 50cm (coordonnés X = 500, Y = 0)
haarkon	7:6d58adc26b78	106	motor.setSpeed (NSpeed,NSpeed);
haarkon	7:6d58adc26b78	107	if (x > 300) {
haarkon	7:6d58adc26b78	108	etatmvt = 1;
haarkon	7:6d58adc26b78	109	}
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	7:6d58adc26b78	113	motor.setSpeed (NSpeed,-NSpeed);
haarkon	7:6d58adc26b78	114	if (theta < (-PI/2.0)) {
haarkon	7:6d58adc26b78	115	etatmvt = 2;
haarkon	7:6d58adc26b78	116	}
haarkon	4:5038b4cd1088	117	break;
haarkon	4:5038b4cd1088	118	case 2 :
haarkon	4:5038b4cd1088	119	// On avance de 50cm (coordonnés X = 500, Y = -500)
haarkon	7:6d58adc26b78	120	motor.setSpeed (NSpeed,NSpeed);
haarkon	7:6d58adc26b78	121	if (y < -300) {
haarkon	7:6d58adc26b78	122	etatmvt = 3;
haarkon	7:6d58adc26b78	123	}
haarkon	4:5038b4cd1088	124	break;
haarkon	4:5038b4cd1088	125	case 3 :
haarkon	4:5038b4cd1088	126	// On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
haarkon	7:6d58adc26b78	127	motor.setSpeed (NSpeed,-NSpeed);
haarkon	7:6d58adc26b78	128	if (theta > 0) {
haarkon	7:6d58adc26b78	129	etatmvt = 4;
haarkon	7:6d58adc26b78	130	}
haarkon	4:5038b4cd1088	131	break;
haarkon	4:5038b4cd1088	132	case 4 :
haarkon	4:5038b4cd1088	133	// On avance de 50cm (coordonnés X = 0, Y = -500)
haarkon	7:6d58adc26b78	134	motor.setSpeed (NSpeed,NSpeed);
haarkon	7:6d58adc26b78	135	if (x < 0) {
haarkon	7:6d58adc26b78	136	etatmvt = 5;
haarkon	7:6d58adc26b78	137	}
haarkon	4:5038b4cd1088	138	break;
haarkon	4:5038b4cd1088	139	case 5 :
haarkon	4:5038b4cd1088	140	// On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
haarkon	7:6d58adc26b78	141	motor.setSpeed (NSpeed,-NSpeed);
haarkon	7:6d58adc26b78	142	if (theta < (PI/2.0)) {
haarkon	7:6d58adc26b78	143	etatmvt = 6;
haarkon	7:6d58adc26b78	144	}
haarkon	4:5038b4cd1088	145	break;
haarkon	4:5038b4cd1088	146	case 6 :
haarkon	4:5038b4cd1088	147	// On avance de 50cm (coordonnés X = 0, Y = 0)
haarkon	7:6d58adc26b78	148	motor.setSpeed (NSpeed,NSpeed);
haarkon	7:6d58adc26b78	149	if (y > 0) {
haarkon	7:6d58adc26b78	150	etatmvt = 7;
haarkon	7:6d58adc26b78	151	}
haarkon	4:5038b4cd1088	152	break;
haarkon	4:5038b4cd1088	153	case 7 :
haarkon	4:5038b4cd1088	154	// On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
haarkon	7:6d58adc26b78	155	motor.setSpeed (NSpeed,-NSpeed);
haarkon	7:6d58adc26b78	156	if (theta < 0) {
haarkon	7:6d58adc26b78	157	etatmvt = 0;
haarkon	7:6d58adc26b78	158	}
haarkon	4:5038b4cd1088	159	break;
haarkon	4:5038b4cd1088	160	}
kyxstark	9:234439133426	161
haarkon	7:6d58adc26b78	162	pc.printf ("\n\n");
haarkon	3:0221daeeaa86	163	led1 = !led1;
haarkon	3:0221daeeaa86	164	led2 = !led2;
kyxstark	8:7b0fa8a914c0	165	wait (1);
haarkon	3:0221daeeaa86	166	}
haarkon	0:4902b3f17137	167	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	06 Jun 2019
Imports:	49
Forks:	0
Commits:	10
Dependents:	0
Dependencies:	2
Followers:	10

main.cpp@9:234439133426, 2019-06-07 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning