SES
サーボのライブラリ. サーボのID, ポジションを指定してシリアル通信でサーボを動かす.
Revision 0:7c7b0148ef4d, committed 2016-06-16
- Comitter:
- SES02
- Date:
- Thu Jun 16 11:29:28 2016 +0000
- Commit message:
- ver 1.0
Changed in this revision
| ICSbus.cpp | Show annotated file Show diff for this revision Revisions of this file |
| ICSbus.h | Show annotated file Show diff for this revision Revisions of this file |
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ICSbus.h Thu Jun 16 11:29:28 2016 +0000
@@ -0,0 +1,23 @@
+#ifndef __ICSbus_H__
+#define __ICSbus_H__
+#include "mbed.h"
+class ICSbus {
+public:
+ ICSbus(PinName pinTX, PinName pinRX);
+ void set_pos(int id, float angle);
+ void set_serial();
+ float pos;
+ int data;
+ int CMD;
+ char pos_H;
+ char pos_L;
+ //int rx_CMD;
+ //char rx_pos_H;
+ //char rx_pos_L;
+ //int counter;
+
+private:
+ Serial serial; //サーボモータとのシリアル通信
+ //Serial pc; //pcとのシリアル通信
+};
+#endif
\ No newline at end of file