/** * @defgroup atlas2000_1 Atlas 20xx (should this exist? vs) * @ingroup bases *@{ */ #ifndef _ATLAS2000_1_CPP_ #define _ATLAS2000_1_CPP_ /** @file atlas2000_1.cpp * @brief atlas2000_1.cpp file for this module. This is where main is defined and etc. */ //#################################################################### // Includes: //#################################################################### #include "atlas2000_1.h" #define bzero(b,len) (memset((b), '\0', (len)), (void) 0) #define DEBUG 0 char *COM_DEVICE_MOTORS; char *COM_DEVICE_SENSORS; int no_safety = FALSE; robot_motion_command_message motion_command; robot_lights_command_message lights_command; robot_vert_sign_command_message vert_sign_command; char send_toogle_msg=0; int PortHandlerMotors, PortHandlerSensors, arguments; double read_frequency, time_elapsed, maximum_dir, minimum_dir; double max_backward_speed,max_forward_speed, wheelaxisdistance; unsigned char cross_sensor; void handler_vert_sign (custom_data_t *); //#################################################################### // Typedefs: required typedefs are declared here. All declared types # // should have TYPE_[the name] ############################### //#################################################################### int Angle_Tranformation(double *X, double *Y, double ang) { double Xmin,Xmax,Ymin,Ymax; int i; int nX=sizeof(X); for(i=0; ilights, atlasmv_command->lights, sizeof(robot_lights_command_msg)); //pthread_mutex_unlock(&command_mutex); }; void handler_lights( custom_data_t *) { } void handler_brake_toggle(custom_data_t*/*data*/) { }; void handler_in(custom_data_t*/*data*/) { in_update++; // if (in_msg->move==1) printf("go\n"); // else printf("!!!!!!!!!!!STOP!!!!!!!!!!!!\n"); usleep(10000); } void handler_mppf(custom_data_t*) { mppf_update++; mppf_dir_update++; usleep(100000); } void handler_motion(custom_data_t *) { //motion_command->speed=motion_command->speed>robot_details->max_forward_speed?robot_details->max_forward_speed:motion_command->speed; //check speed limits atlas2000_command->motion->speed=((atlas2000_command->motion->speed > atlas2000_details.max_forward_speed)?atlas2000_details.max_forward_speed:atlas2000_command->motion->speed); atlas2000_command->motion->speed=((atlas2000_command->motion->speed < atlas2000_details.max_backward_speed)?atlas2000_details.max_backward_speed:atlas2000_command->motion->speed); //check direction values atlas2000_command->motion->dir=((atlas2000_command->motion->dir > atlas2000_details.maximum_dir)?atlas2000_details.maximum_dir:atlas2000_command->motion->dir); atlas2000_command->motion->dir=((atlas2000_command->motion->dir < atlas2000_details.minimum_dir)?-atlas2000_details.minimum_dir:atlas2000_command->motion->dir); //printf("lock_command_mutex handler\n"); //printf("unlock_command_mutex handler\n"); } /** * @brief thread function that will communicate with DES maxon servoamplifier * * @return void */ void *des_thread_func(void *) { #define DES_VELOCITY_TYPE 1 //velocity can be 0 - mean value or 1 - instant c_timer timer_des; timer_des.tic(0); double actual_speed; double new_speed; new_speed = 0.00; while(1) { timer_des.tic(9); timer_des.toc(0); //printf("des_status:%d\n", atlasmv->get_des_initialized()); if( (!atlas2000->get_des_initialized()) ) { // printf("DES:error time:%f\n", lar_get_time()); // if(timer_des.get_toc(0)>0.02) if(timer_des.get_toc(0)>0.1) { atlas2000->check_des(); pthread_mutex_lock(&count_des_errors_mutex); count_des_errors++; //printf("tried to re-arm maxon servoAmplifier:%d\n", count_des_errors); pthread_mutex_unlock(&count_des_errors_mutex); timer_des.tic(0); } }else{ pthread_mutex_lock(&count_des_errors_mutex); count_des_errors=0; pthread_mutex_unlock(&count_des_errors_mutex); } //get new speed turn on flag pthread_mutex_lock(&command_mutex); if(running_order1) { new_speed = running_order1->motion.speed; } // IPC_listen(0); if (in_update) //initial navigation { if(in_msg->move==1 && mppf_msg->orient_ok==0) new_speed=0.9; else { // printf("in_msg->move %d,mppf_msg->orient_ok %d\n",in_msg->move,mppf_msg->orient_ok); new_speed=0; // in_update=FALSE; // VER QUE SE PASSA AO TIRAR ISTO igual para o mppf_update } } // printf("\nin_update %d\n",in_update); if(mppf_update && mppf_msg->orient_ok==1) //mppf speed & orientation { if(mppf_msg->move==1) { new_speed=0.8; } if(mppf_msg->move==0) { new_speed=0; // mppf_update=FALSE; } } pthread_mutex_unlock(&command_mutex); //conversion to have velocity is made. turn on flag timer_des.tic(2); atlas2000->read_speed(DES_VELOCITY_TYPE); timer_des.toc(2); // printf("get speed time:%f %s\n", timer_des.get_toc(2),a==0?"FALSE":"TRUE"); atlas2000->get_linear_speed(&actual_speed, NULL); if(CheckBrakeToggle()) new_speed=0; // printf("new_speed=%3.2f\n",new_speed); // new_speed=.5; atlas2000->set_speed(&new_speed); timer_des.toc(9); //printf("DES:new-speed:%1.2f actual_speed:%1.2f. freq_actual:%2.0f\n", new_speed, actual_speed, (1.0/timer_des.get_toc(9))); //usleep(1000);//is required a sleep to avoid collisions between threads //usleep(15000); timer_des.run_sleep(9, 20);//requires this thread to run at a maximal of 30 Hz } return NULL; }; bool CheckBrakeToggle(void) { if(brake_toggle==NULL) return false; if(brake_toggle->toggle==0) return false; if(atlas2000->get_cross_sensor()==0) return false; return true; } /** * @brief thread function to interface communication with DIOC board * * @return void */ void *dioc_thread_func(void *) { c_timer timer_dioc; robot_lights_command_message *lights_msg; robot_vert_sign_command_message *vert_sign_msg; lights_msg = new robot_lights_command_message; vert_sign_msg = new robot_vert_sign_command_message; memset( lights_msg, NULL, sizeof(robot_lights_command_message)); memset( vert_sign_msg, NULL, sizeof(robot_vert_sign_command_message)); while(1) { timer_dioc.tic(0); pthread_mutex_lock(&command_mutex); if(running_order1) { memcpy( lights_msg, &running_order1->lights, sizeof(robot_lights_command_message)); memcpy( vert_sign_msg, &running_order1->vert_sign, sizeof(robot_vert_sign_command_message)); //printf("vert_sign:%d | hl:%d, rl:%d, tl:%d, L:%d, R:%d\n", vert_sign_msg->vert_sign, lights_msg->headlights, lights_msg->reverselights, lights_msg->taillights, lights_msg->turnleft, lights_msg->turnright); } pthread_mutex_unlock(&command_mutex); atlas2000->set_lights( vert_sign_msg, lights_msg); //cross-walk detection request_cross_sensor_value(); read_cross_sensor_value(); atlas2000->read_cross_sensor(cross_sensor); //atlas2000_status.cross_sensor = cross_sensor; timer_dioc.toc(0); timer_dioc.run_sleep(0, 20); } return NULL; }; /** * @brief * * @return */ void *servos_thread_func(void *) { double actual_speed, req_speed=0; double new_brake=0; unsigned char Identificator,msg,msgsent = 0;; int n, i, pwm; double m_dir=2.347; double b_dir=43.657; double dir=0; // double X[]={0, 10, 20, 30, 40, 45, 50, 60, 70, 80, 85, 90}; // double Y[]={-17.5, -13.9, -9.9, -5, -1.75, 0, 2.5, 4.7, 9 ,15.5, 18.25, 22}; c_timer timer_servo; //new values for m & b, not tested m_dir=2.32299; b_dir=42.57141; while(1) { timer_servo.tic(0); pthread_mutex_lock(&command_mutex); if(running_order1) { dir = running_order1->motion.dir; req_speed = running_order1->motion.speed; } pthread_mutex_unlock(&command_mutex); if (dir < minimum_dir) dir = minimum_dir; if (dir > maximum_dir) dir = maximum_dir; pwm=m_dir*dir*(180/M_PI)+b_dir; //valores retirados do codigo antigo // pwm = -1.28*motion_command.dir + 45.44; // pwm = 5*(dir*180/M_PI) + 43.; // pwm=((85.-43.)/(double)maximum_dir)*dir+43; // printf("dir=%f pwm=%d\n",dir*(180/M_PI),pwm); if (dir==0) pwm=45; if(mppf_dir_update) //mppf direction { m_dir=-0.44536; b_dir=45; pwm=m_dir*mppf_msg->dir+b_dir; } // if(mppf_dir_update && !mppf_msg->radial) //mppf direction // { // m_dir=-0.44536; // b_dir=45; // pwm=m_dir*mppf_msg->dir+b_dir; // if(mppf_msg->dir<-90) pwm=90;//equerda // else // if(mppf_msg->dir<=-45 && mppf_msg->dir>=-90) pwm=70/*90*/; // else // if(mppf_msg->dir<=-15 && mppf_msg->dir>=-45) pwm=55/*90*/; // else // if(mppf_msg->dir>-15 && mppf_msg->dir<15) pwm=45;//centro // else // if(mppf_msg->dir>=15 && mppf_msg->dir<=45) pwm=35/*0*/;//direita // else // if(mppf_msg->dir>=45 && mppf_msg->dir<=90) pwm=20/*0*/;//direita // else // if (mppf_msg->dir>90) pwm=0; // mppf_dir_update=FALSE; // } // if(mppf_dir_update && mppf_msg->radial) //mppf_radial direction // { // m_dir=0.44536; // b_dir=45; // pwm=m_dir*mppf_msg->dir+b_dir; // if(mppf_msg->dir < -45) pwm=0; //direita // if(mppf_msg->dir >= -45 && mppf_msg->dir <= 45) pwm=m_dir*mppf_msg->dir+b_dir; // if(mppf_msg->dir > 45) pwm=90; //esquerda // printf("mppf_msg->dir %d pwm %d\n",mppf_msg->dir,pwm); // mppf_r_dir_update=FALSE; // } if(pwm>90)pwm=90; if(pwm<0)pwm=0; if(flags.debug) printf("Desired direction: %3.2f, computed PWM: %d \n", motion_command.dir, pwm); if(!flags.dummy) { Identificator = 0; msg=(Identificator | pwm); for(i = 0; i < 3; i++) //now to send both values { n = write( PortHandlerMotors, &msg , sizeof(msg) ); if( n < 0) { perror("Direction and Speed to PIC1: Failed\n"); carmen_ipc_sleep(1); } else { if(flags.debug) printf("Direction and Speed to PIC1: Sent\n"); msgsent=1; } if (msgsent) break; } } atlas2000->set_actual_dir(&dir); atlas2000->get_linear_speed(&actual_speed, NULL); //brake analysis if( (fabs(req_speed) < fabs(actual_speed)) ) { if( fabs(req_speed-actual_speed) > atlas2000->get_brake_speed_dif() ) { new_brake=1.0; // printf("braking speed is higher\n"); } else{ new_brake=0.0; } }else{ new_brake=0.0; } timer_servo.toc(0); if(CheckBrakeToggle() && actual_speed>0.05) { // printf("Forcing brake\n"); new_brake=1; } //atlas2000->set_brake(&new_brake); set_brake((int)new_brake); timer_servo.run_sleep(0, 20); } return NULL; }; void set_brake(int brake) { unsigned char msg; unsigned char brakemsg; unsigned char Identificator; unsigned char msgsent = 0; int i,n; if (brake==1) brakemsg= 103; else brakemsg= 104; Identificator = 0b10000000; msg=(Identificator | brakemsg); for(i = 0; i < 3; i++) //now to send both values { n = write( PortHandlerMotors, &msg , sizeof(msg) ); if( n < 0) { perror("Direction and Speed to PIC1: Failed\n"); carmen_ipc_sleep(1); } else { if(flags.debug) printf("Direction and Speed to PIC1: Sent\n"); msgsent=1; // printf("brake\n"); } if (msgsent) break; } } void atlas2000_shut(int x) { pthread_cancel(des_thread); pthread_cancel(dioc_thread); pthread_cancel(servos_thread); delete atlas2000; atlas2000_shutdown_module(x); }; static void initialize_robot(void) { if(!flags.dummy) { //opens port for PIC1 struct termios ComParams, newParams; int ret; //opens port for PIC2 bzero( &newParams, sizeof(newParams)); //seting all structure values to zero printf("Opening comm Lights on device = %s", COM_DEVICE_SENSORS);fflush(stdout); PortHandlerSensors = open( COM_DEVICE_SENSORS, O_RDWR | O_NONBLOCK ); //open com device. read only and non blocking modes printf("Done\n"); if(PortHandlerSensors == -1) //if could not open { printf("Failed on opening Port %s for COM_DEVICE_SENSORS\n",COM_DEVICE_SENSORS); exit(1); } ret=tcgetattr(PortHandlerSensors, &ComParams); // get serial communuication parameters if( ret < 0 ) perror("Get serial communication parameters failed:"); newParams.c_cflag = B9600 | CS8 | CLOCAL | CREAD; newParams.c_iflag = IGNPAR; newParams.c_oflag = 0; ret=tcsetattr( PortHandlerSensors, TCSANOW, &newParams ); // set serial communuication parameters if( ret < 0 ) perror("Set serial communication parameters failed:"); bzero( &newParams, sizeof(newParams)); //seting all structure values to zero printf("Opening comm Motor...");fflush(stdout); PortHandlerMotors = open( COM_DEVICE_MOTORS, O_RDWR | O_NONBLOCK ); //open com device. read only and non blocking modes printf("Done\n"); if(PortHandlerMotors == -1) //if could not open { printf("Failed on opening Port %s for Motors\n",COM_DEVICE_MOTORS); exit(1); } ret=tcgetattr(PortHandlerMotors, &ComParams); // get serial communuication parameters if( ret < 0 ) perror("Get serial communication parameters failed:"); newParams.c_cflag = B9600 | CS8 | CLOCAL | CREAD; newParams.c_iflag = IGNPAR; newParams.c_oflag = 0; ret=tcsetattr( PortHandlerMotors, TCSANOW, &newParams ); // set serial communuication parameters if( ret < 0 ) perror("Set serial communication parameters failed:"); printf("\nROBOT INITIALIZED\n"); } else printf("DUMMY INITIALIZED\n\n"); } void atlas_lights_command(void) { char msgToSend=0; int n=0,i; if(flags.debug) { printf("fl = %d,tl = %d,tr = %d,bl = %d,rl = %d\n",lights_command.headlights, lights_command.turnleft, lights_command.turnright, lights_command.taillights, lights_command.reverselights); } if(!flags.dummy) { msgToSend = (msgToSend | (lights_command.turnright<<3 )); msgToSend = (msgToSend | (lights_command.turnleft<<2)); msgToSend = (msgToSend | (lights_command.headlights<<4)); msgToSend = (msgToSend | lights_command.taillights<<1); msgToSend = (msgToSend | (lights_command.reverselights)); for(i=0;i < 3;i++) { n = write( PortHandlerSensors, &msgToSend , 1 ); if( n < 0) { perror("Lights Command to PIC2: Failed\n"); carmen_ipc_sleep(10); //wait 10 ms(?? confirm) until next attemp } else { if(flags.debug) printf("Lights Command to PIC2: Sent\n"); break; } } } } /** @fn void atlas_vert_sign_command(void) *@brief function that commands the lights controller about vertical sign choice *@param void. *@return void - . */ void atlas_vert_sign_command(void) { static int previous_sign = NO_VERT_SIGN; unsigned char msgToSend=0; int n=0,i; if(previous_sign != vert_sign_command.vert_sign) { if(flags.debug) { printf("vert_sign = %d\n", vert_sign_command.vert_sign); printf("Sending new vertsign command\n"); } switch (vert_sign_command.vert_sign) { case MANDATORY_MEDIUM_LIGTHS:{ msgToSend = 0b10000001; break; } case MANDATORY_BUS_LANE:{ msgToSend = 0b10000010; break; } case WARNING_DIP_AHEAD:{ msgToSend = 0b10000100; break; } case WARNING_ROAD_NARROWS:{ msgToSend = 0b10001000; break; } case INFORMATION_HOSPITAL:{ msgToSend = 0b10010000; break; } case INFORMATION_REC_SPEED_60:{ msgToSend = 0b10100000; break; } } if(!flags.dummy) { for(i=0;i < 3;i++) { n = write( PortHandlerSensors, &msgToSend , 1 ); if( n < 0) { perror("Vertical Sign Command to PIC2: Failed\n"); carmen_ipc_sleep(10); //wait 10 ms(?? confirm) until next attemp } else { if(flags.debug) printf("Vertical Sign Command to PIC2: Sent\n"); break; } } } previous_sign = vert_sign_command.vert_sign; vert_sign_command.vert_sign = 0; }else { if(flags.debug) { printf("vert_sign = %d\n", vert_sign_command.vert_sign); printf("vertsign command equal. not send\n"); } } } void request_cross_sensor_value(void) { unsigned char msg, Identificator; int n, i; unsigned char msgsent=0; if(flags.debug) printf("Requesting Cross Sensor Value: \n"); if(!flags.dummy) { Identificator=1; Identificator=Identificator<<7; msg=Identificator; for(i=0;i< 4;i++) //now to send both values { n = write( PortHandlerSensors, &msg , sizeof(msg) ); if( n < 0) { perror("Requesting Cross Sensor Value: Failed\n"); } else { if(flags.debug) printf("Requesting Cross Sensor Value: Sent\n"); msgsent=1; } if (msgsent) break; } } } void read_cross_sensor_value(void) { int n; unsigned char byteread[20]; if(!flags.dummy) { n = read(PortHandlerSensors, &byteread,20); if( n == -1 ) { perror("Failed reading from device--------------------------------------------------"); } if (n>0) cross_sensor = byteread[n-1]; if (cross_sensor == 65) cross_sensor = 1; else cross_sensor = 0; if(flags.debug) { fprintf(stderr,"\r\t\t\t\t\t\t\t\t\tXs = %d", cross_sensor); printf("Cross Sensor is %d\n",byteread[n-1]); } } } void safety_handler(void) { motion_command.timestamp = carmen_get_time(); } void read_frequency_change_handler() { if(flags.debug) printf("\nRead frequency changed, new value = %.2f \n",read_frequency); } void shutdown_module(int x) { if(x == SIGINT) { if(!flags.dummy) { close(PortHandlerMotors); close(PortHandlerSensors); } carmen_ipc_disconnect(); printf("\33[36base_atlas\33[0m: Disconnected.\n"); exit(1); } } c_timer timer;//added by dgameiro on 26-02-2010 double ts;//added by dgameiro on 26-02-2010 int cycle_freq, low_freq;//added by dgameiro on 26-02-2010 carmen_param_t module_refresh_params[] = { {(char*)"freq", (char*)"base", CARMEN_PARAM_INT, &cycle_freq, 1, NULL}, };//added by dgameiro on 26-02-2010 carmen_param_t basesensor_params[] = { {(char*)"robot", (char*)"frequency", CARMEN_PARAM_DOUBLE, &read_frequency, 1, NULL}, {(char*)"robot", (char*)"comport_motors", CARMEN_PARAM_STRING, &COM_DEVICE_MOTORS, 0, NULL}, {(char*)"robot", (char*)"comport_sensors", CARMEN_PARAM_STRING, &COM_DEVICE_SENSORS, 0, NULL}, {(char*)"robot", (char*)"maximum_dir", CARMEN_PARAM_DOUBLE, &maximum_dir, 1, NULL}, {(char*)"robot", (char*)"minimum_dir", CARMEN_PARAM_DOUBLE, &minimum_dir, 1, NULL}, {(char*)"robot", (char*)"wheelaxisdistance", CARMEN_PARAM_DOUBLE, &wheelaxisdistance, 1, NULL}, {(char*)"robot", (char*)"max_forward_speed", CARMEN_PARAM_DOUBLE, &max_forward_speed, 1, NULL}, {(char*)"robot", (char*)"max_backward_speed", CARMEN_PARAM_DOUBLE, &max_backward_speed, 1, NULL}}; int main(int argc, char **argv) { atlas2000_command = new t_atlasmv_command; readcommandlineparameters(argc, argv, &flags, 0); //start by reading command line parameters // atlasmv_help(&flags); signal(SIGINT, atlas2000_shut); //set the handler for signals. Will handle ctrl+c (SIGINT) with a clean exit atlas2000_details.com_servos = (char*)malloc(256*sizeof(char)); atlas2000_details.com_des = (char*)malloc(256*sizeof(char)); atlas2000_details.com_dioc = (char*)malloc(256*sizeof(char)); storage = cvCreateMemStorage(0); motion_queue = cvCreateSeq(0, sizeof(CvSeq), sizeof(t_motion_queued), storage); atlas2000_details.sleep_time = 1.0/atlas2000_details.cycle_freq; //use a wrapped carmen function, defined in the atlasmv_interface.cpp to obtain the parameters values for //the structure public_parameters, that was also declared in the header file as a "TYPE_atlasmv_public_params" //atlasmv_param_daemon_install_public_parameters(¶ms, 1, 1); //alloc memory for msg pointers. Usually, local binary memory is allocated for the worst case scenario. //Sometimes not all allocated memory is passed in the message. #define colorred //Printing Startup message. After this, nothing can be written to the shell unless were using debug mode. // atlasmv_status= new robot_status_message; // atlas2000_status= new robot_status_message; atlas2000_command = new t_atlasmv_command; // memset(atlas2000_command,0,t_atlasmv_command); atlas2000_command->motion = new robot_motion_command_message; memset(atlas2000_command->motion ,0,sizeof(robot_motion_command_message)); atlas2000_command->vert_sign = new robot_vert_sign_command_message; memset(atlas2000_command->vert_sign ,0,sizeof(robot_vert_sign_command_message)); atlas2000_command->lights = new robot_lights_command_message; memset(atlas2000_command->lights ,0,sizeof(robot_lights_command_message)); // ipc_atlasmv = new t_ipc_atlasmv; atlas2000 = new c_atlas2000(&flags); //initialize each thread //use a carmen function to obtain the parameters values for //the structure atlasmv_local_parameters, that was declared in the header file carmen_ipc_initialize(argc, argv); //Init IPC carmen_param_check_version(argv[0]);//Check Param version carmen_param_install_params(argc, argv, module_refresh_params, sizeof(module_refresh_params)/ sizeof(module_refresh_params[0])); int ret = carmen_param_install_params(argc, argv, basesensor_params, sizeof(basesensor_params)/ sizeof(basesensor_params[0])); if (ret==-1){printf("could not read startup vars from param_daemon. Aborting\n");exit(0);} atlasmv_param_daemon_install_public_parameters(&atlas2000_details, 1, NULL, flags.debug); initialize_motion_model(&motion_model, 0, 0, 0, atlas2000_details.wheelaxisdistance); running_order.motion.speed=0.0; running_order.motion.dir=0.0; running_order.motion.brake=0.0; atlas2000->set_atlasmv_details(&atlas2000_details); initialize_robot(); int initialize = atlas2000->initialize_robot(); if(!(initialize & 1)) { //printf("init:%d", initialize); if(initialize & 2) printf("\33[31mError with DIOC\33[0m\n"); if(initialize & 4) printf("\33[31mError with ServoAmplifier\33[0m\n"); if(initialize & 8) printf("\33[31mError with Servos\33[0m\n"); atlas2000_shutdown_module(SIGINT); } atlas2000_status.host=carmen_get_host(); des_iret = pthread_create(&des_thread, NULL, des_thread_func , NULL); servos_iret = pthread_create(&servos_thread, NULL, servos_thread_func , NULL); dioc_iret = pthread_create(&dioc_thread, NULL, dioc_thread_func , NULL); ipc_atlasmv.pub_status = new class_ipc_wrapper(ROBOT_STATUS_NAME, ROBOT_STATUS_FMT, IPC_WRAPPER_PUBLISH,&(atlas2000_status)); ipc_atlasmv.sub_motion = new class_ipc_wrapper(ROBOT_MOTION_COMMAND_NAME, ROBOT_MOTION_COMMAND_FMT, IPC_WRAPPER_SUBSCRIBE_LATEST, false, handler_motion, NULL, (void **)&(atlas2000_command->motion)); ipc_atlasmv.sub_vert_sign = new class_ipc_wrapper(ROBOT_VERT_SIGN_COMMAND_NAME, ROBOT_VERT_SIGN_COMMAND_FMT, IPC_WRAPPER_SUBSCRIBE_LATEST, false, handler_vert_sign, NULL, (void **)&(atlas2000_command->vert_sign)); ipc_atlasmv.sub_lights = new class_ipc_wrapper(ROBOT_LIGHTS_COMMAND_NAME, ROBOT_LIGHTS_COMMAND_FMT, IPC_WRAPPER_SUBSCRIBE_LATEST, false, handler_lights, NULL, (void **)&(atlas2000_command->lights)); ipc_atlasmv.sub_brake_toggle = new class_ipc_wrapper(ROBOT_BRAKE_TOGGLE_NAME,ROBOT_BRAKE_TOGGLE_FMT, IPC_WRAPPER_SUBSCRIBE_LATEST, false, handler_brake_toggle, NULL, (void **)&(brake_toggle)); printf("\33[36matlasmv\33[0m: publishing \33[31m%s\33[0m\n",ATLASMV_DATA_MESSAGE_NAME); in_data = new class_ipc_wrapper(IN_DATA_MESSAGE_NAME , IN_DATA_MESSAGE_FMT, IPC_WRAPPER_SUBSCRIBE_LATEST, false, handler_in, NULL, (void **)&(in_msg)); mppf_data = new class_ipc_wrapper(MPPF_DATA_MESSAGE_NAME , MPPF_DATA_MESSAGE_FMT, IPC_WRAPPER_SUBSCRIBE_LATEST, false, handler_mppf, NULL, (void **)&(mppf_msg)); timer.tic(1); while (1) { // printf("motion %f\n", motion_command.dir); //timer.set_ipc_sleep() set_motion_queue(motion_queue, atlas2000_command); pthread_mutex_lock(&command_mutex); running_order1 = check_motion_queue(motion_queue); // running_order1->motion = running_order->motion; //running_order1 pthread_mutex_unlock(&command_mutex); atlas2000->update_robot_status(&atlas2000_status); update_motion_model(&motion_model, atlas2000_status.speed, atlas2000_status.dir); timer.toc(1); increment_motion_model(&motion_model, timer.get_toc(1)); //printf("taxa:%.3f\n", timer.get_toc(1)); timer.tic(1); atlas2000_status.x = motion_model.X; atlas2000_status.y = motion_model.Y; atlas2000_status.orientation = motion_model.HA; atlas2000_status.distance_traveled = 2*motion_model.S; //printf("S %f D %f E %d C: %d\n",atlasmv_status->speed,atlasmv_status->dir, atlasmv_status->errors, atlasmv_status->cross_sensor); //printf("X:%.2f Y:%.2f HA:%.2f S:%.4f\n", atlasmv_status->x, atlasmv_status->y, carmen_radians_to_degrees(atlasmv_status->orientation),atlasmv_status->distance_traveled); // timer.verb(1,4); atlas2000_status.timestamp = lar_get_time(); ipc_atlasmv.pub_status->publish_message();//publica a mensagem // if(atlas2000_status.distance_traveled==0) // printf("distance_traveled=%f\n",atlas2000_status.distance_traveled); // publish_status(&motion_command,&lights_command,&vert_sign_command, cross_sensor); lar_ipc_sleep(0.1); //carmen_ipc_sleep(atlasmv_details.sleep_time); //lar_ipc_sleep(0.2); } } #endif /** *@} */