File content as of revision 22:bf5aa20b9df0:

#include <sstream>

#include "Console.h"

Console::Console(PinName tx, PinName rx)
{
    serial = new Serial(tx, rx);
}

Console::~Console()
{
    delete serial;
}

uint8_t Console::getheader()
{
    uint8_t comm = getc_nowait();
    if (comm != 0) {
        serial->printf("Mode:%c \r\n", comm);
    }
    return comm;
}

int Console::getid()
{
    int id;
    stringstream sstr;
    serial->printf("Enter the ID: \r\n");
    sstr << getc_wait();
    sstr >> id;
    serial->printf("Servo ID:%d \r\n", id);
    return id;
}

uint16_t Console::get_int_cr()
{
    stringstream sstr;
    serial->printf("Enter the Servo value. And push Enter.: \r\n");
    while (1) {
        uint8_t tmp = getc_wait();
        serial->printf("%c", tmp);
        if (tmp == '\r') {
            serial->printf("\r\n");
            break;
            }
        sstr << tmp;
    }
    uint16_t val;
    sstr >> val;
    serial->printf("Servo value:%d \r\n", val);
    return val;
}

uint8_t Console::getc_wait()
{
    while (!serial->readable());
    uint8_t val = serial->getc();
    return val;
}

uint8_t Console::getc_nowait()
{
    uint8_t buf = 0;
    if (serial->readable()) {
        buf = serial->getc();
    }
    return buf;
}

void Console::printf(char* str)
{
    serial->printf(str);
}

/*uint16_t Console::readint()
{
    uint8_t buff[2];
    buff[0] = getc_wait();
    buff[1] = getc_wait();
    uint16_t val;
    val = (uint16_t)(buff[1] << 8) | (uint16_t)buff[0];
    return val;
}*/

/*void sendint(uint16_t val)
{
    uint8_t buff[16];
    buff[0] = (uint8_t)(val & 0x00FF);
    buff[1] = (uint8_t)(val >> 8);
    pc.putc(buff[0]);
    pc.putc(buff[1]);
}*/