Esempio n. 1
0
/*
 * Encoder publisher node
 */
msg_t encoder_node(void *arg) {
	encoder_node_conf * conf = (encoder_node_conf *) arg;
	r2p::Node node(conf->name);
	r2p::Publisher<r2p::EncoderMsg> enc_pub;
	systime_t time;
	qeidelta_t delta;
	r2p::EncoderMsg *msgp;

	(void) arg;
	chRegSetThreadName(conf->name);

	/* Enable the QEI driver. */
	qeiInit();
	qeiStart(&QEI_DRIVER, &qeicfg);
	qeiEnable (&QEI_DRIVER);

	node.advertise(enc_pub, conf->topic);

	for (;;) {
		time = chTimeNow();
		delta = qeiUpdate(&QEI_DRIVER);

		if (enc_pub.alloc(msgp)) {
			msgp->delta = delta * conf->t2r;
			enc_pub.publish(*msgp);
		}

		time += MS2ST(20);
		chThdSleepUntil(time);
	}

	return CH_SUCCESS;
}
Esempio n. 2
0
msg_t encoder_node(void *arg) {
	Node node("encoder");
	Publisher<tEncoderMsg> enc_pub;
	systime_t time;
	tEncoderMsg *msgp;

	(void) arg;
	chRegSetThreadName("encoder");

	qeiStart(&QEI_DRIVER, &qeicfg);
	qeiEnable (&QEI_DRIVER);

	node.advertise(enc_pub, "encoder"MOTOR_ID_STRING);

	for (;;) {
		time = chTimeNow();

		if (enc_pub.alloc(msgp)) {
			msgp->timestamp.sec = chTimeNow();
			msgp->timestamp.nsec = chTimeNow();
			msgp->delta = T2R(qeiUpdate(&QEI_DRIVER) * 100);
			enc_pub.publish(*msgp);
		}

		time += MS2ST(10);
		chThdSleepUntil(time);
	}

	return CH_SUCCESS;
}
Esempio n. 3
0
msg_t qeipub_node(void *arg) {
	Node node("qeipub");
	Publisher<tQEIMsg> qei_pub;
	systime_t time;
	tQEIMsg *msgp;

	(void) arg;
	chRegSetThreadName("qeipub");

	qeiStart(&QEI_DRIVER, &qeicfg);
	qeiEnable (&QEI_DRIVER);

	node.advertise(qei_pub, "qei"MOTOR_ID_STRING);

	for (;;) {
		time = chTimeNow();
		int16_t delta = qeiUpdate(&QEI_DRIVER);

		if (qei_pub.alloc(msgp)) {
			msgp->timestamp.sec = 0;
			msgp->timestamp.nsec = 0;
			msgp->delta = delta;
			qei_pub.publish(*msgp);
		}

		time += MS2ST(50);
		chThdSleepUntil(time);
	}

	return CH_SUCCESS;
}
Esempio n. 4
0
msg_t encoder_node(void *arg) {
	Node node("encoder");
	Publisher<tEncoderMsg> enc_pub;
	systime_t time;
	tEncoderMsg *msgp;

	(void) arg;
	chRegSetThreadName("encoder");

	qeiStart(&QEI_DRIVER, &qeicfg);
	qeiEnable (&QEI_DRIVER);


	switch (stm32_id8()) {
	case M1:
		node.advertise(enc_pub, "encoder1");
		break;
	case M2:
		node.advertise(enc_pub, "encoder2");
		break;
	case M3:
		node.advertise(enc_pub, "encoder3");
		break;
	default:
		node.advertise(enc_pub, "encoder");
		break;
	}

	for (;;) {
		time = chTimeNow();

		if (enc_pub.alloc(msgp)) {
			msgp->timestamp.sec = chTimeNow();
			msgp->timestamp.nsec = chTimeNow();
			msgp->delta = T2R(qeiUpdate(&QEI_DRIVER) * 100);
			enc_pub.publish(*msgp);
		}

		time += MS2ST(10);
		chThdSleepUntil(time);
	}

	return CH_SUCCESS;
}
Esempio n. 5
0
msg_t qeipub_node(void *arg) {
	Node node("qeipub");
	Publisher<tQEIMsg> qei_pub;
	systime_t time;
	tQEIMsg *msgp;

	(void) arg;
	chRegSetThreadName("qeipub");

	qeiStart(&QEI_DRIVER, &qeicfg);
	qeiEnable (&QEI_DRIVER);

	switch (stm32_id8()) {
	case M1:
		node.advertise(qei_pub, "qei1");
		break;
	case M2:
		node.advertise(qei_pub, "qei2");
		break;
	case M3:
		node.advertise(qei_pub, "qei3");
		break;
	default:
		node.advertise(qei_pub, "qei");
		break;
	}

	for (;;) {
		time = chTimeNow();
		int16_t delta = qeiUpdate(&QEI_DRIVER);

		if (qei_pub.alloc(msgp)) {
			msgp->timestamp.sec = 0;
			msgp->timestamp.nsec = 0;
			msgp->delta = delta;
			qei_pub.publish(*msgp);
		}

		time += MS2ST(50);
		chThdSleepUntil(time);
	}

	return CH_SUCCESS;
}