João Pedro Castilho / Mbed OS Lab4

lab 4

Dependencies:   BufferedSerial

Communication.cpp

Committer:
ccpjboss
Date:
2021-05-27
Revision:
0:0d3a25d4697e

File content as of revision 0:0d3a25d4697e:

#include "Communication.h"
#include "mbed.h"
#include "MessageBuilder.h"

Serial *serial_object;
MessageBuilder bin_msg;

void init_communication(Serial *serial_in)
{
    serial_object = serial_in;
}

void write_bytes(char *ptr, unsigned char len)
{
    for(int i=0; i<len; i++)
    {
        serial_object->putc(ptr[i]);
    }
}

void send_odometry(float x, float y, float theta)
{
    bin_msg.reset();
    bin_msg.add('O');

    bin_msg.add(x);
    bin_msg.add(y);
    bin_msg.add(theta);

    write_bytes(bin_msg.message, bin_msg.length());
}

void send_map(int x, int y, float map)
{
    bin_msg.reset();
    bin_msg.add('M');

    bin_msg.add(x);
    bin_msg.add(y);
    bin_msg.add(map);

    write_bytes(bin_msg.message, bin_msg.length());
}