publisher.c
Go to the documentation of this file.
00001 /***************************************************************************
00002 ***  Fraunhofer IPA 
00003 ***  Robotersysteme 
00004 ***  Projekt: 3D cartography demonstrator
00005 ****************************************************************************
00006 ****************************************************************************
00007 ***  Autor: Julio Sagardoy (goa-js)
00008 ***************************************************************************/
00009 
00017 
00018 #include <avr/io.h>
00019 
00021 #include <string.h>
00022 #include <stdlib.h>
00023 #include <stdint.h>
00024 
00026 #include <encoder.h>
00027 #include <stepper.h>
00028 #include <servo.h>
00029 #include <usart.h>
00030 #include <publisher.h>
00031 #include <utils.h>
00032 
00033 int8_t publishData()
00034 {
00035         char* p_pos_str;
00036         char* e_pos_str;
00037         char* t_pos_str;
00038         char* message;
00039 
00040         // allocate memory for the strings that will be used
00041         p_pos_str = malloc(sizeof(char) * 5 + 1);
00042         e_pos_str = malloc(sizeof(char) * 5 + 1);
00043         t_pos_str = malloc(sizeof(char) * 5 + 1);
00044         message = malloc(sizeof(char) * 32 + 1);
00045 
00046         // check for correct memory allocation
00047         if( (p_pos_str || e_pos_str || t_pos_str || message ) == '\0' )
00048                 return false;
00049         
00050         // initialise first position of pointers to null, so strcat can work well
00051         *p_pos_str = '\0';
00052         *e_pos_str = '\0';
00053         *t_pos_str = '\0';
00054         *message = '\0';
00055 
00056         // read current positions       
00057         int16_t p_pos = stepper_read();
00058         int16_t e_pos = encoder_read();
00059         int16_t t_pos = servo_read();
00060 
00061         // convert integers to strings
00062         itoa( p_pos, p_pos_str, 10);
00063         itoa( e_pos, e_pos_str, 10);
00064         itoa( t_pos, t_pos_str, 10);
00065 
00066         // create message to output
00067         strcat(message, "R:");
00068         strcat(message, p_pos_str);
00069         strcat(message, ",");
00070         strcat(message, e_pos_str);
00071         strcat(message, ",");
00072         strcat(message, t_pos_str);
00073 
00074         // send message
00075         uart_puts(message);
00076 
00077         // deallocate memory
00078         free(p_pos_str);
00079         free(e_pos_str);                
00080         free(t_pos_str);                
00081         free(message);
00082         
00083         return true;
00084 }


cob_3d_mapping_demonstrator
Author(s): Georg Arbeiter
autogenerated on Wed Aug 26 2015 11:03:46