first
Dependents: 17robo_fuzi 17robo_tokyo_kaede
Diff: rs422_put.cpp
- Revision:
- 0:2365b54fd0db
diff -r 000000000000 -r 2365b54fd0db rs422_put.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/rs422_put.cpp Sun Sep 24 05:24:26 2017 +0000 @@ -0,0 +1,61 @@ +#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; + } +} \ No newline at end of file