SES
サーボのライブラリ. サーボのID, ポジションを指定してシリアル通信でサーボを動かす.
Diff: ICSbus.cpp
- Revision:
- 0:7c7b0148ef4d
--- /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