Diff: rs422_put.cpp

Revision:

0:2365b54fd0db

--- /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