C++ (Cpp) T2Rの例

コード例 #1

ファイル: hardware_test.cpp プロジェクト: r2p-old/uDC_module

msg_t encoder_node(void *arg) {
	r2p::Node node("encoder");
	r2p::Publisher<r2p::EncoderMsg> enc_pub;
	systime_t time;
	qeidelta_t delta;
	r2p::EncoderMsg *msgp;

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

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

	node.advertise(enc_pub, "encoder");

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

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

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

	return CH_SUCCESS;
}

コード例 #2

ファイル: motor.cpp プロジェクト: r2p/Middleware

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;
}

コード例 #3

ファイル: motor.cpp プロジェクト: AIRLab-POLIMI/BasketBot_old

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;
}