main.cpp

00001 #include "mbed.h"
00002 #include "pb.h"
00003 #include "pb_encode.h"
00004 #include "pb_decode.h"
00005 #include "threeaxis.pb.h"
00006 #include "MODSERIAL.h"
00007 
00008 MODSERIAL pc(USBTX, USBRX, "modser"); //modified modser lib to be able to input custom stream
00009 
00010 int main() {
00011     pc.baud(115200);
00012     gyro_message GyroOut, GyroIn;
00013     
00014     pc.claim();
00015     uint8_t bufferout[150];
00016     uint8_t bufferin[150];
00017     
00018     pb_ostream_t streamout = pb_ostream_from_buffer(bufferout, sizeof(bufferout));
00019 
00020     GyroOut.X=1.1;
00021     GyroOut.Y=2.1;
00022     GyroOut.Z=3.1;
00023     pc.printf("starting..\r\n");
00024     while(1){
00025         GyroOut.X+=0.1;
00026         GyroOut.Y+=0.2;
00027         GyroOut.Z+=0.3;
00028         
00029         pc.printf("Raw values: x: %4.2f, y: %4.2f, z: %4.2f\r\n", GyroOut.X, GyroOut.Y, GyroOut.Z); //print values before encoding
00030         
00031         if (pb_encode(&streamout, gyro_message_fields, &GyroOut)) { //encode message
00032             /*pc.putc('$');
00033             //fwrite(buffer, 1, stream.bytes_written, stdout);
00034             pc.printf("%s", buffer);
00035             pc.putc('e');
00036             pc.putc('n');
00037             pc.putc('d');
00038             //pc.printf("x: %4.2f, y: %4.2f, z: %4.2f\r\n", Gyro.X, Gyro.Y, Gyro.Z);
00039             //stream*=0; //empty stream, not working as of now but creating error on mbed
00040             //memset(&buffer[0], 0, sizeof(buffer)); //empty buffer*/
00041 
00042             //new test code:
00043             
00044             pc.printf("%s\r\n", bufferout);
00045         }
00046         
00047         else { //print error message if encoding fails
00048             pc.printf("Encoding failed: %s\n", PB_GET_ERROR(&streamout));
00049             return 0;
00050         }
00051         
00052         pc.printf("decoding...\r\n");
00053         pb_istream_t streamin = pb_istream_from_buffer(bufferin, sizeof(bufferin)); //create input stream
00054         for(int i=0;i<=streamout.bytes_written;i++) //copy output buffer to input buffer
00055             bufferin[i]=bufferout[i];
00056         if (pb_decode(&streamin, gyro_message_fields, &GyroIn)) { //decode message
00057             pc.printf("Decoded values: x: %4.2f, y: %4.2f, z: %4.2f\r\n", GyroIn.X, GyroIn.Y, GyroIn.Z); //print decoded values
00058         }
00059         
00060         else { //print error message if decoding fails
00061             pc.printf("Decoding failed: %s\n", PB_GET_ERROR(&streamin));
00062             return 0;
00063         }
00064         wait(1);
00065     }
00066 }