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));
*/
}
