Diff: ICSbus.cpp

Revision:

0:7c7b0148ef4d

diff -r 000000000000 -r 7c7b0148ef4d ICSbus.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ICSbus.cpp	Thu Jun 16 11:29:28 2016 +0000
@@ -0,0 +1,35 @@
+#include "mbed.h"
+#include "ICSbus.h"
+ICSbus::ICSbus(PinName pinTX, PinName pinRX) : 
+serial(pinTX, pinRX)
+{}
+
+void ICSbus::set_pos(int id, float angle) {
+    float pos = 0;
+    int data = 0; //3500～11500
+    angle += 135;
+    pos = (8000 / 270) * angle;
+    data = (int)(pos > 0.5 ? pos + 0.5 : pos - 0.5);
+    data += 3500;
+    
+    int CMD = 0b10000000 + id;
+    char pos_H = (uint16_t)((data & 0b11111110000000) >> 7);
+    char pos_L = (uint16_t)(data & 0b00000001111111);
+    
+    serial.putc(CMD);
+    serial.putc(pos_H);
+    serial.putc(pos_L);
+    //int rx_CMD = serial.getc(); counter++;
+    //char rx_pos_H = serial.getc(); counter++;
+    //char rx_pos_L = serial.getc(); counter++;
+    
+    //while (counter <= 3);
+    //counter = 0;
+}
+    
+void ICSbus::set_serial(){
+    serial.baud(115200);
+    serial.format(8, Serial::Even, 1);
+    //pc.baud(115200);
+    serial.format(8, Serial::Even, 1);
+}
\ No newline at end of file