first
Dependents: 17robo_fuzi 17robo_tokyo_kaede
rs422_put.cpp
- Committer:
- echo_piyo
- Date:
- 2017-09-24
- Revision:
- 1:dd594521d6e2
- Parent:
- 0:2365b54fd0db
File content as of revision 1:dd594521d6e2:
#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; } }