void app_initialize(void) {
static const UrosString turtle1 = { 7, "turtle1" };
static const UrosNodeConfig *const cfgp = &urosNode.config;
unsigned i;
/* Initialize the uROS system.*/
urosInit();
fifo_init(&rosoutQueue, 8);
/* Initialize the turtle slots.*/
urosMutexObjectInit(&turtleCanSpawnLock);
turtleCanSpawn = UROS_TRUE;
turtle_init_pools();
for (i = 0; i < MAX_TURTLES; ++i) {
turtle_init(&turtles[i], i);
}
/* Create the Node thread.*/
urosNodeCreateThread();
/* Spawn the first turtle.*/
turtle_spawn(&turtle1, 0.5f * SANDBOX_WIDTH, 0.5f * SANDBOX_HEIGHT, 0.0f);
}
void app_initialize(void) {
uros_err_t err;
(void)err;
urosMutexObjectInit(&benchmark.lock);
benchmark.rate = 1;
urosStringObjectInit(&benchmark.payload);
memset(&benchmark.inCount, 0, sizeof(streamcnt_t));
memset(&benchmark.outCount, 0, sizeof(streamcnt_t));
memset(&benchmark.curCpu, 0, sizeof(cpucnt_t));
memset(&benchmark.oldCpu, 0, sizeof(cpucnt_t));
urosInit();
urosNodeCreateThread();
err = urosThreadCreateStatic(&benchmark.printerId, "printer", 50,
app_printer_thread, NULL, printerstack,
PRINTER_STKLEN);
urosAssert(err == UROS_OK);
}
/*
* Application entry point.
*/
int main(void) {
Thread *shelltp = NULL;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
palClearPad(LED1_GPIO, LED1);
palClearPad(LED2_GPIO, LED2);
palClearPad(LED3_GPIO, LED3);
palClearPad(LED4_GPIO, LED4);
chThdSleepMilliseconds(500);
palSetPad(LED1_GPIO, LED1);
palSetPad(LED2_GPIO, LED2);
palSetPad(LED3_GPIO, LED3);
palSetPad(LED4_GPIO, LED4);
/*
* Activates the serial driver 1 using the driver default configuration.
*/
sdStart(&SERIAL_DRIVER, NULL);
r2p::Middleware::instance.initialize(wa_info, sizeof(wa_info), r2p::Thread::LOWEST);
rtcantra.initialize(rtcan_config);
r2p::Middleware::instance.start();
/* Make the PHY wake up.*/
palSetPad(GPIOC, GPIOC_ETH_NOT_PWRDN);
/* Creates the LWIP thread (it changes priority internally).*/
chThdCreateStatic(wa_lwip_thread, THD_WA_SIZE(LWIP_THREAD_STACK_SIZE), NORMALPRIO + 5, lwip_thread, NULL);
chThdSleepMilliseconds(100);
r2p::ledsub_conf ledsub_conf = { "led" };
r2p::Thread::create_heap(NULL, THD_WA_SIZE(256), NORMALPRIO, r2p::ledsub_node, &ledsub_conf);
//Setup subscribers
//current_node.subscribe(current_sub, "bits_packed");
//current_node.set_enabled(true);
vel_node.advertise(vel_pub, "current2", r2p::Time::INFINITE);
vel_node.set_enabled(true);
urosInit();
urosNodeCreateThread();
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the button state.
*/
while (TRUE) {
r2p::Thread::sleep(r2p::Time::s(20));
}
}