File content as of revision 0:2365b54fd0db:

#include "rs422_put.h"

Rs422::Rs422(PinName mbed_tx, PinName mbed_rx) : RS422(mbed_tx,mbed_rx)
{
}

void Rs422::begin(int baudrate)
{
    RS422.baud(baudrate);
    timer.attach(this, &Rs422::put_time, wait_time);
}

void Rs422::put(short id, float m1_duty, float m2_duty)
{
    if (flag == 0) {
        ID = id;
        if (m1_duty < 0) {
            m1_data = 0b10000000;
        } else {
            m1_data = 0;
        }
        if (m2_duty < 0) {
            m2_data = 0b10000000;
        } else {
            m2_data = 0;
        }
        m1_data += fabs(m1_duty) * 100;
        m2_data += fabs(m2_duty) * 100;

        check_sum = (id + m1_data + m2_data) & 0b01111111;
    }
    flag = 1;
}

void Rs422::put_time()
{
    switch (counter) {
        case 0 :
            RS422.putc(start_signal);
            break;
        case 1 :
            RS422.putc(ID);
            break;
        case 2 :
            RS422.putc(m1_data);
            break;
        case 3 :
            RS422.putc(m2_data);
            break;
        case 4 :
            RS422.putc(check_sum);
            break;
        default :
            break;
    }
    counter ++;
    if(counter >= 5) {
        counter = 0;
        flag = 0;
    }
}