Diff: B3MServo.cpp

Revision:

0:d784ce1cfa3d

Child:

1:8077f2887cb1

Child:

2:89e3b7b3ca11

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/B3MServo.cpp	Mon May 28 06:31:56 2018 +0000
@@ -0,0 +1,59 @@
+#include "B3MServo.h"
+
+//https://kondo-robot.com/faq/b3m_softwaremanual1200
+
+/*sample program*********************************************
+
+#include "mbed.h"
+#include "B3MServo.h"
+
+int main()
+{
+    B3MServo myServo(p13, p14, p12);
+    myServo.onTorque(0);
+    while(1)
+    {
+        myServo.controlAngle(0, 90);
+        wait_ms(2500);
+        myServo.controlAngle(0, -90);
+        wait_ms(2500);
+    }
+}
+
+************************************************************/
+
+B3MServo::B3MServo(PinName tx, PinName rx, PinName enable) : BufferedSerial(tx, rx), _enable(enable)
+{
+    BufferedSerial::baud(115200);
+}
+
+void B3MServo::sendPacket(char command, char id, char addr, int data, char size, char option, char count)
+{
+    _enable = 1;
+    char packet[0x09] = {};
+    
+    packet[0] = size+7;
+    packet[1] = command;
+    packet[2] = option;
+    packet[3] = id;
+    for(int i = 0; i < size; i++)
+        packet[4+i] = char( ( data >> (i*8) ) & 0xFF );
+    packet[size+4] = addr;
+    packet[size+5] = count;
+    packet[size+6] = 0;
+    for(int i = 0; i < packet[0]-1; i++)
+        packet[size+6] += packet[i];
+        
+    BufferedSerial::write(packet, packet[0]);
+    wait_us(1500);
+    _enable = 0;
+}
+
+void B3MServo::onTorque(char id, char movingMode, char controlMode)
+{
+    sendPacket(WRITE, id, TorqueON, int((controlMode << 2) | movingMode));
+}
+void B3MServo::controlAngle(char id, float angle)
+{
+    sendPacket(WRITE, id, DesiredPosition, int(angle*100));
+}
\ No newline at end of file