void Odo(void *arg){ double rtick_prev = 0; double ltick_prev = 0; double dtick_r = 0; double dtick_l = 0; char r_mesg[40], l_mesg[40]; int r_tick, l_tick; double Dr, Dc, Dl, x, y, theta, x_dt, y_dt, theta_dt, x_new, y_new, theta_new; Dr = Dc = Dl = 0; x = y = theta = 0; x_new=y_new=theta_new = 0; x_dt = y_dt = theta_dt = 0; rt_task_set_periodic(NULL, TM_NOW, period); while(1){ rt_task_wait_period(NULL); rt_queue_read(&rqueue, r_mesg, sizeof(r_mesg), TM_INFINITE); rt_queue_read(&lqueue, l_mesg, sizeof(l_mesg), TM_INFINITE); r_tick = atoi(r_mesg); l_tick = atoi(l_mesg); //rt_printf("int data: %d \n", l_tick); //rt_printf("data r : %d \n", r_tick); dtick_r = r_tick - rtick_prev; dtick_l = l_tick - ltick_prev; Dr = m_per_tick*dtick_r; Dl = m_per_tick*dtick_l; Dc = (Dr+Dl)/2; x_dt = Dc*cos(theta); y_dt = Dc*sin(theta); theta_dt = (Dr-Dl)/L; theta_new = theta + theta_dt; x_new = x + x_dt; y_new = y + y_dt; rtick_prev = r_tick; ltick_prev = l_tick; x = x_new; y = y_new; theta = theta_new; rt_printf("Robot pose (x, y, theta) is: %lf, %lf, %lf\n", x, y, theta); } }
/* private functions ======================================================== */ static void task_soft_routine(void * cookie) { unsigned long n = 0; /* how many wheel rotations since init? */ RTIME time_init = 0; /* when did we start? */ RTIME time_prev = 0; /* when was the previous rotation? */ RTIME time_curr = 0; /* when was the current rotation? */ /* we start now! (first wheel rotation) */ rt_queue_read(&queue, &time_init, sizeof time_curr, TM_INFINITE); /* previous rotation is now! (init value) */ time_prev = time_init; while (1) { /* extract the current rotation timestamp from message queue */ rt_queue_read(&queue, &time_curr, sizeof time_curr, TM_INFINITE); /* Let's compute instant speed. We want an Hz value, we've got previous rotation timestamp and current rotation timestamp, substracting them give us the time elapsed since previous rotation in nanoseconds. Hz = 1 / s, so Hz = 10^9 / ns, our final value is 10^9 / dt */ rt_mutex_acquire(&mutex_instant, TM_INFINITE); instant = 1e9 / (time_curr - time_prev); rt_mutex_release(&mutex_instant); /* Let's compute average speed. We want an Hz value, we've got init rotation timestamp and current rotation timestamp, substracting them gives us the time elapsed since init rotation in nanoseconds. ++n is the number of wheel rotations since init. Hz = 1 / s, so Hz = 10^9 / ns, our final value is 10^9 * dx / dt */ rt_mutex_acquire(&mutex_average, TM_INFINITE); average = 1e9 * ++n / (time_curr - time_init); rt_mutex_release(&mutex_average); /* dump current timestamp to file */ fprintf(ostream, "%llu\n", time_curr); /* our job is done, we are now the previous rotation */ time_prev = time_curr; } (void) cookie; }
void taskTwo(void *arg) { int retval; char msgBuf[MAX_MESSAGE_LENGTH]; /* receive message */ retval = rt_queue_read(&myqueue,msgBuf,sizeof(msgBuf),TM_INFINITE); if (retval < 0 ) { rt_printf("Receiving error\n"); } else { rt_printf("taskTwo received message: %s\n",msgBuf); rt_printf("with length %d\n",retval); } }
void envoyer(void * arg) { DMessage *msg; int err; while (1) { rt_printf("tenvoyer : Attente d'un message\n"); if ((err = rt_queue_read(&queueMsgGUI, &msg, sizeof (DMessage), TM_INFINITE)) >= 0) { rt_printf("tenvoyer : envoi d'un message au moniteur\n"); serveur->send(serveur, msg); msg->free(msg); } else { rt_printf("Error msg queue write: %s\n", strerror(-err)); } } }
Matrix periodic_function(Matrix* inputChannel,short numChannels){ char buf[CHAR_BUFFER_SIZE]; struct debugframe temp; static unsigned long long epoch = 0; int i,ret=1; while(ret){ ret = rt_queue_read(io.debug_queue,&temp,sizeof(temp),TM_NONBLOCK); //If queue has no more messages to process //or has been deleted and hence become invalid if(ret == -EWOULDBLOCK || ret == -EINVAL) break; if(!epoch) epoch = temp.start_time; LOG("\nBlock name : %s\n",temp.config_file); LOG("Block type : %s\n",temp.block_name); LOG("Start : %llu.%06llu us\n", (temp.start_time - epoch) / 1000, (temp.start_time - epoch) % 1000); LOG("End : %llu.%06llu us\n", (temp.end_time - epoch) / 1000, (temp.end_time - epoch) % 1000); LOG("Inputs : %d\n",temp.input_num); for(i=0;i<temp.input_num;i++){ matrix_print_pretty(&temp.input[i],temp.input_name[i],NULL); matrix_string(&temp.input[i], &buf); fprintf(log,"- %-11s: %s\n",temp.input_name[i],buf); } matrix_string(&temp.output, &buf); LOG("Output : %s\n",buf); } }
/** * * @param oncillaPtr */ void executor(void *oncillaPtr) { int ret; unsigned long overrun; OncillaCmd msgBuf; OncillaRobot *oncilla = static_cast<OncillaRobot*> (oncillaPtr); sbcp::ScheduledWorkflow & w = oncilla->bus->Scheduled(); for (int i = 0; i < 4; i++) { w.AppendScheduledDevice(std::tr1::static_pointer_cast<sbcp::Device, sbcp::amarsi::MotorDriver>(oncilla->legs[i])); oncilla->legs[i]->Motor1().GoalPosition().Set(oncilla->zeroPosRaw[2 * i]); oncilla->legs[i]->Motor2().GoalPosition().Set(oncilla->zeroPosRaw[2 * i + 1]); oncilla->servos[i]->SetCommand(oncilla->zeroPosRaw[i + 8]); } ret = rt_task_set_periodic(NULL, TM_NOW, rt_timer_ns2ticks(oncilla->executor_task_period_ns)); if (ret) { std::runtime_error ex("Xenomai task set periodic failed. Error"); throw ex; } ret = rt_task_set_mode(0, T_PRIMARY, NULL); if (ret) { std::runtime_error ex("Xenomai task set mode failed. Error"); throw ex; } RTIME start_time = rt_timer_read(), current_time; while (true) { rt_mutex_acquire(&(oncilla->end_executor), TM_INFINITE); //rt_mutex_release(&(oncilla->end_executor)); ret = rt_task_wait_period(&overrun); if (ret) { std::runtime_error ex("Xenomai task wait failed. Error"); //throw ex; } ret = rt_queue_read(&(oncilla->posQueue), &msgBuf, sizeof (msgBuf), TM_NONBLOCK); if (ret >= 0) { if (msgBuf.id == OncillaCmd::SET_POS) { oncilla->legs[0]->Motor1().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[0] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[0] / 2.0)); oncilla->legs[0]->Motor2().GoalPosition().Set((int16_t) ((1.0 - msgBuf.args.doubleArgs[1]) * oncilla->maxMotorPos[1])); oncilla->legs[1]->Motor1().GoalPosition().Set((int16_t) (-msgBuf.args.doubleArgs[2] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[2] / 2.0)); oncilla->legs[1]->Motor2().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[3] * oncilla->maxMotorPos[3])); oncilla->legs[2]->Motor1().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[4] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[4] / 2.0)); oncilla->legs[2]->Motor2().GoalPosition().Set((int16_t) ((1.0 - msgBuf.args.doubleArgs[5]) * oncilla->maxMotorPos[5])); oncilla->legs[3]->Motor1().GoalPosition().Set((int16_t) (-msgBuf.args.doubleArgs[6] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[6] / 2.0)); oncilla->legs[3]->Motor2().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[7] * oncilla->maxMotorPos[7])); for (int i = 0; i < 4; i++) oncilla->servos[i]->SetCommand(msgBuf.args.doubleArgs[i + 8]); rt_queue_free(&(oncilla->posQueue), &msgBuf); } else if (msgBuf.id == OncillaCmd::RESET_TIMER) { if (oncilla->RT) { rt_mutex_acquire(&(oncilla->posArrMutexRT), TM_INFINITE); oncilla->trajectQueue.clear(); rt_mutex_release(&(oncilla->posArrMutexRT)); } else { pthread_mutex_lock(&(oncilla->posArrMutex)); oncilla->trajectQueue.clear(); pthread_mutex_unlock(&(oncilla->posArrMutex)); rt_task_set_mode(0, T_PRIMARY, NULL); } start_time = rt_timer_read(); } } else if (ret == -EINVAL || ret == -EIDRM) { break; } else if (ret != -EWOULDBLOCK) { std::runtime_error ex("Cannot read command from message queue. Error"); throw ex; } try { w.StartTransfers(); w.WaitForTransfersCompletion(); } catch (sbcp::MultipleTransferError & e) { std::runtime_error ex("SBCP communication failed. Error"); throw ex; } if (oncilla->RT) { rt_mutex_acquire(&(oncilla->posArrMutexRT), TM_INFINITE); } else { // If causing timeout for real-time task try using pthread_mutex_trylock() instead pthread_mutex_lock(&(oncilla->posArrMutex)); } for (int i = 0; i < 4; i++) { oncilla->currentPosRaw[2 * i] = oncilla->legs[i]->Motor1().PresentPosition().Get(); oncilla->currentPosRaw[2 * i + 1] = oncilla->legs[i]->Motor2().PresentPosition().Get(); } oncilla->currentPos[0] = (double) (oncilla->currentPosRaw[0] - oncilla->maxMotorPos[0] / 2.0) / 4096.0 * (2.0 * M_PI); oncilla->currentPos[1] = (1.0 - (double) oncilla->currentPosRaw[1] / (double) oncilla->maxMotorPos[1]); oncilla->currentPos[2] = (double) -(oncilla->currentPosRaw[2] - oncilla->maxMotorPos[2] / 2.0) / 4096.0 * (2.0 * M_PI); oncilla->currentPos[3] = ((double) oncilla->currentPosRaw[3] / (double) oncilla->maxMotorPos[3]); oncilla->currentPos[4] = (double) (oncilla->currentPosRaw[4] - oncilla->maxMotorPos[4] / 2.0) / 4096.0 * (2.0 * M_PI); oncilla->currentPos[5] = (1.0 - (double) oncilla->currentPosRaw[5] / (double) oncilla->maxMotorPos[5]); oncilla->currentPos[6] = (double) -(oncilla->currentPosRaw[6] - oncilla->maxMotorPos[6] / 2.0) / 4096.0 * (2.0 * M_PI); oncilla->currentPos[7] = ((double) oncilla->currentPosRaw[7] / (double) oncilla->maxMotorPos[7]); for (int i = 0; i < 4; i++) oncilla->currentPos[i + 8] = oncilla->servos[i]->Command(); current_time = rt_timer_read(); OncillaRobot::trajectPoint newTrajectPoint; newTrajectPoint.coords[0] = (double) (current_time - start_time) / 1000000; memcpy(newTrajectPoint.coords + 1, oncilla->currentPos, 12 * sizeof (double)); if (oncilla->trajectQueue.size() >= oncilla->queueSize) oncilla->trajectQueue.pop_front(); oncilla->trajectQueue.push_back(newTrajectPoint); if (oncilla->RT) { rt_mutex_release(&(oncilla->posArrMutexRT)); } else { pthread_mutex_unlock(&(oncilla->posArrMutex)); rt_task_set_mode(0, T_PRIMARY, NULL); } rt_mutex_release(&(oncilla->end_executor)); //rt_mutex_acquire(&(oncilla->end_executor), TM_INFINITE); } /*rt_mutex_release(&(oncilla->end_executor)); pthread_mutex_lock(&(oncilla->end_mutex)); pthread_cond_signal(&(oncilla->cond)); pthread_mutex_unlock(&(oncilla->end_mutex)); */ }