File content as of revision 0:08d5f31c6ddc:

#include "MatlabComm.h"

MatlabComm::MatlabComm(PinName MotorDirectionPin, PinName MotorSpeedPin, PinName EncoderChannelA, PinName EncoderChannelB) : 
    M_SpeedPin(MotorSpeedPin), M_DirectionPin(MotorDirectionPin), M_Encoder(EncoderChannelA, EncoderChannelB, NC, 64, QEI::X4_ENCODING)
    , pc(USBTX,USBRX)    
{

    pc.baud(115200);
    pc.printf("Hello\r\n");
    buffer_index=0;
}


// write data to the output.
void MatlabComm::write(){
    pc.printf("%i\r\n", M_Encoder.getPulses()+10);
};

void MatlabComm::flush_buffer(){
    for (int i=0; i < buffer_index; i++){
        char_buffer[i]=0;   
    }
    buffer_index=0;
}

void MatlabComm::read(){
    while (pc.readable()){
        char get = pc.getc();
        char_buffer[buffer_index] = get;
        buffer_index++;
        
        // Check for a number.
        if (get == '\n'){
            
            double num;
            int result;
            result = sscanf(char_buffer,"%lf", &num); // Retrieve char from array;
            if (result){
                M_DirectionPin = (int)(num > 0);
                M_SpeedPin = abs(num);
                flush_buffer();
            }
            else { // other input, flush buffer.
                flush_buffer();
            }

        }
        
        
        if (buffer_index>buffer_max_index){
            flush_buffer();   
        }
    }
}

void MatlabComm::Step(){
    write();
    read();
}