Diff: LSCServo.cpp

Revision:

0:3b51e0118f2b

Child:

1:1398d9ddd4ef

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSCServo.cpp	Wed May 19 09:51:20 2021 +0000
@@ -0,0 +1,122 @@
+//mbed library
+#include "mbed.h"
+//ros library
+#include "BufferedSerial.h"
+
+#include <vector>
+#include "LSCServo.h"
+
+#define GET_LOW_char(A) (uint8_t)((A))
+//Macro function  get lower 8 bits of A
+#define GET_HIGH_char(A) (uint8_t)((A) >> 8)
+//Macro function  get higher 8 bits of A
+#define char_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B))
+//put A as higher 8 bits   B as lower 8 bits   which amalgamated into 16 bits integer
+
+using namespace std;
+
+
+//constructor
+LSCServo::LSCServo(PinName tx, PinName rx, uint32_t buf_size, uint32_t tx_multiple, const char* name) : serial(tx, rx, buf_size, tx_multiple)
+{
+}
+//destructor
+LSCServo::~LSCServo()
+{
+}
+void LSCServo::LobotSerialoneServoMove(uint16_t id, int16_t position, uint16_t time) //control one serial servo move dedicated position
+{
+    char buf[10];
+    if(position < 0)
+    position = 0;
+    if(position > 1000)
+    position = 1000;
+    buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
+    buf[2] = 0x08;
+    buf[3] = 0x03;
+    buf[4] = 0x01;
+    buf[5] = GET_LOW_char(time);
+    buf[6] = GET_HIGH_char(time);
+    buf[7] = GET_LOW_char(id);
+    buf[8] = GET_LOW_char(position);
+    buf[9] = GET_HIGH_char(position);
+    serial.write(buf, 10);
+}
+void LSCServo::LobotSerialServoMove(vector<uint16_t> id, vector<int16_t> position, uint16_t time) //control list of serial servo move dedicated position
+{
+    char buf[126];
+    int limit = id.size();
+    int checksum = limit*3 + 6;
+    buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
+    buf[2] = GET_LOW_char(checksum-2);
+    buf[3] = 0x03;
+    buf[4] = GET_LOW_char(limit);
+    buf[5] = GET_LOW_char(time);
+    buf[6] = GET_HIGH_char(time);
+    int index = 7;
+    for(int i = 0; i < limit; i++)
+    {
+        buf[index]=GET_LOW_char(id[i]);
+        index++;
+        if(position[i] < 0)
+            position[i] = 0;
+        if(position[i] > 1000)
+            position[i] = 1000;
+        buf[index]=GET_LOW_char(position[i]);
+        index++;
+        buf[index]=GET_HIGH_char(position[i]);
+        index++;
+    }
+    serial.write(buf,checksum);
+}
+void LSCServo::LobotSerialActionGroupRun(uint16_t gpid, uint16_t count) //run action group(action group id, repeat time)
+{    
+    char buf[7];
+    buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
+    buf[2]=0x05;
+    buf[3]=0x06;
+    buf[4]=GET_LOW_char(gpid);
+    buf[5]=GET_LOW_char(count);
+    buf[6]=GET_HIGH_char(count);
+    serial.write(buf,7);
+}
+void LSCServo::LobotSerialActionGroupSpeed(uint16_t gpid, uint16_t speed) //Set Run speed Action Group in XXX%
+{
+    char buf[7];
+    buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
+    buf[2]=0x05;
+    buf[3]=0x0B;
+    buf[4]=GET_LOW_char(gpid);
+    buf[5]=GET_LOW_char(speed);
+    buf[6]=GET_HIGH_char(speed);
+    serial.write(buf,7);
+}
+int LSCServo::LobotSerialGetBatteryVoltage() //get voltage of board in terms of mV
+{
+    char buf[4];
+    buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
+    buf[2]=0x02;
+    buf[3]=0x0F;
+    serial.write(buf,4);
+    while(serial.readable()==0)
+    {
+        ;
+    }
+    MyBuffer<char> rx = serial.getc();
+    int limit = rx.getSize();
+    char* word;
+    for(int i = 0; i < limit; i++)
+    {
+        *(word + i) = rx;
+    }
+    int voltage = char_TO_HW(*(word + 5), *(word + 4));
+    return voltage;
+}
+void LSCServo::LobotSerialActionGroupStop() //Stop current action group
+{
+    char buf[4];
+    buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
+    buf[2]=0x02;
+    buf[3]=0x07;
+    serial.write(buf,4);
+}
\ No newline at end of file