![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
sampleProgram
Dependencies: QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo
Fork of 17robo_fuzi by
Diff: RS422_put.h
- Revision:
- 0:bf96e953cdb8
diff -r 000000000000 -r bf96e953cdb8 RS422_put.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RS422_put.h Mon Jun 26 09:59:14 2017 +0000 @@ -0,0 +1,85 @@ +/*************** +RS422 master libraly + +RS422 name(tx, rx) +tx : mbed_tx +rx : mbed_rx + +put(int id, float m1_duty, float m2_duty) +id : 0 - 254 +duty : -1.0 - 1.0 + +****************/ + + +#define start_signal 0b11111111 +#define cool_time 0.0004 + +class RS422 +{ +public : + RS422(PinName mbed_tx, PinName mbed_rx) : Rs422(mbed_tx,mbed_rx) { + } + + void begin(int baudrate) { + Rs422.baud(baudrate); + timer.attach(this, &RS422::put_time, cool_time); + } + + void put(int 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; + } + +private : + Serial Rs422; + Ticker timer; + + short flag; + int counter; + int m1_data,m2_data,check_sum; + int ID; + + void 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