Diff: main.cpp

Revision:

0:07a5cc567526

diff -r 000000000000 -r 07a5cc567526 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Mar 27 07:53:20 2021 +0000
@@ -0,0 +1,79 @@
+#include "mbed.h"
+#include "BufferedSerial.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
+#define ID1 1
+#define ID2 2
+#define ID3 3
+#define ID4 4
+
+#define LOBOT_SERVO_FRAME_HEADER         0x55
+#define LOBOT_SERVO_MOVE_TIME_WRITE      1
+
+using namespace std;
+
+BufferedSerial serial_port(PA_9, PA_10, 16, 16); // tx, rx, buffer size, multiple
+
+DigitalOut myled(LED1);
+
+char LobotCheckSum(char buf[])
+{
+  char i;
+  uint16_t temp = 0;
+  for (i = 2; i < buf[3] + 2; i++) {
+    temp += buf[i];
+  }
+  temp = ~temp;
+  i = (char)temp;
+  return i;
+}
+
+
+void LobotSerialoneServoMove(uint8_t id, int16_t position, uint16_t time)
+{
+  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] = id;
+  buf[8] = GET_LOW_char(position);
+  buf[9] = GET_HIGH_char(position);
+  serial_port.write(buf, 10);
+}
+
+
+int main() {
+    //serial_port.set_baud(115200);
+    //serial_port.set_format(8,BufferedSerial::None,1);
+    //char buf[10]={0};
+    while(1){
+        myled = 1;
+        LobotSerialoneServoMove(ID1, 700, 1000);//id, pos, time
+        LobotSerialoneServoMove(ID2, 500, 1000);
+        wait_ms(1000);
+        LobotSerialoneServoMove(ID1, 500, 1000);
+        LobotSerialoneServoMove(ID2, 600, 1000);
+        wait_ms(1000);
+        LobotSerialoneServoMove(ID1, 900, 1000);
+        LobotSerialoneServoMove(ID2, 700, 1000);
+        wait_ms(1000);
+        LobotSerialoneServoMove(ID1, 500, 1000);
+        LobotSerialoneServoMove(ID2, 600, 1000);
+        wait_ms(1000);
+        myled = 0;
+        wait_ms(1000);
+    }
+}
\ No newline at end of file