Diff: RS300.cpp

Revision:

0:6b230fd13b40

Child:

1:64e11d4c49ae

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RS300.cpp	Wed Sep 11 18:29:27 2013 +0000
@@ -0,0 +1,58 @@
+#include "RS300.h"
+
+RS300::RS300(PinName tx, PinName rx) : serial(tx, rx) {
+    serial.baud(115200);
+}
+
+void RS300::send_packet(uint8_t adr, std::vector<uint8_t> &data, uint8_t cnt, uint8_t id, uint8_t flag)
+{
+    std::vector<uint8_t> buf;
+    buf.push_back(0xFA);//Header :H
+    buf.push_back(0xAF);//Header :L
+    buf.push_back(id);  //id
+    buf.push_back(flag);//flag
+    buf.push_back(adr); //adress
+    buf.push_back(data.size() / cnt);//data size
+
+    buf.push_back(cnt);//amount of servo
+    buf.insert(buf.end(), data.begin(), data.end());//data
+
+    uint8_t sum = buf[2];//check sum from id to data
+    for (int i = 3, size = buf.size(); i < size; ++i) {
+        sum = sum ^ buf[i];
+    }
+    buf.push_back(sum);
+
+    for (std::vector<uint8_t>::iterator i = buf.begin(); i != buf.end(); ++i) {
+        serial.putc(*i);
+    }
+}
+
+void RS300::on_all_servo()
+{
+    std::vector<uint8_t> dat;
+    dat.push_back(0x01);
+    send_packet(0x24, dat, 1, 0xFF);//adress, on, servo amount, id(all)
+}
+
+void RS300::off_all_servo()
+{
+    std::vector<uint8_t> dat;
+    dat.push_back(0x00);
+    send_packet(0x24, dat, 1, 0xFF);//adress, off, servo amount, id(all)
+}
+
+void RS300::send_servo_pos(uint8_t id_s, std::vector<uint16_t> &pos)
+{
+    std::vector<uint8_t> buf;
+    {
+        uint8_t id = id_s;
+        std::vector<uint16_t>::iterator i = pos.begin();
+        for ( ; i != pos.end(); ++i, ++id) {
+            buf.push_back(id);
+            buf.push_back((uint8_t)((*i) & 0x00FF));
+            buf.push_back((uint8_t)((*i) >> 8));
+        }
+    }
+    send_packet(0x1E, buf, pos.size());
+}
\ No newline at end of file