Robocon 2021 / LSCServo

The function library of serial servo controller

Dependents:   DR-ArmServoTest ros_button_2021 DR-ArmServoTest

LSCServo.cpp@1:1398d9ddd4ef, 2021-05-25 (annotated)

Committer:
howanglam3
Date:
Tue May 25 09:18:49 2021 +0000
Revision:
1:1398d9ddd4ef
Parent:
0:3b51e0118f2b
Child:
2:acaaf1126d76
complete recv

Who changed what in which revision?

UserRevisionLine numberNew contents of line
howanglam3 0:3b51e0118f2b 1 //mbed library
howanglam3 0:3b51e0118f2b 2 #include "mbed.h"
howanglam3 0:3b51e0118f2b 3 //ros library
howanglam3 0:3b51e0118f2b 4 #include "BufferedSerial.h"
howanglam3 0:3b51e0118f2b 5
howanglam3 0:3b51e0118f2b 6 #include <vector>
howanglam3 0:3b51e0118f2b 7 #include "LSCServo.h"
howanglam3 0:3b51e0118f2b 8
howanglam3 0:3b51e0118f2b 9 #define GET_LOW_char(A) (uint8_t)((A))
howanglam3 0:3b51e0118f2b 10 //Macro function get lower 8 bits of A
howanglam3 0:3b51e0118f2b 11 #define GET_HIGH_char(A) (uint8_t)((A) >> 8)
howanglam3 0:3b51e0118f2b 12 //Macro function get higher 8 bits of A
howanglam3 0:3b51e0118f2b 13 #define char_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B))
howanglam3 0:3b51e0118f2b 14 //put A as higher 8 bits B as lower 8 bits which amalgamated into 16 bits integer
howanglam3 0:3b51e0118f2b 15
howanglam3 0:3b51e0118f2b 16 using namespace std;
howanglam3 0:3b51e0118f2b 17
howanglam3 0:3b51e0118f2b 18 //constructor
howanglam3 0:3b51e0118f2b 19 LSCServo::LSCServo(PinName tx, PinName rx, uint32_t buf_size, uint32_t tx_multiple, const char* name) : serial(tx, rx, buf_size, tx_multiple)
howanglam3 0:3b51e0118f2b 20 {
howanglam3 0:3b51e0118f2b 21 }
howanglam3 0:3b51e0118f2b 22 //destructor
howanglam3 0:3b51e0118f2b 23 LSCServo::~LSCServo()
howanglam3 0:3b51e0118f2b 24 {
howanglam3 0:3b51e0118f2b 25 }
howanglam3 1:1398d9ddd4ef 26
howanglam3 1:1398d9ddd4ef 27 void LSCServo::receive_buffer(char* destination, int length)
howanglam3 1:1398d9ddd4ef 28 {
howanglam3 1:1398d9ddd4ef 29 int index = 0;
howanglam3 1:1398d9ddd4ef 30 while(index < length)
howanglam3 1:1398d9ddd4ef 31 {
howanglam3 1:1398d9ddd4ef 32 while(!serial.readable());
howanglam3 1:1398d9ddd4ef 33 while(serial.readable())
howanglam3 1:1398d9ddd4ef 34 {
howanglam3 1:1398d9ddd4ef 35 destination[index++] = serial.getc();
howanglam3 1:1398d9ddd4ef 36 }
howanglam3 1:1398d9ddd4ef 37 }
howanglam3 1:1398d9ddd4ef 38 }
howanglam3 1:1398d9ddd4ef 39
howanglam3 0:3b51e0118f2b 40 void LSCServo::LobotSerialoneServoMove(uint16_t id, int16_t position, uint16_t time) //control one serial servo move dedicated position
howanglam3 0:3b51e0118f2b 41 {
howanglam3 0:3b51e0118f2b 42 char buf[10];
howanglam3 0:3b51e0118f2b 43 if(position < 0)
howanglam3 0:3b51e0118f2b 44 position = 0;
howanglam3 0:3b51e0118f2b 45 if(position > 1000)
howanglam3 0:3b51e0118f2b 46 position = 1000;
howanglam3 0:3b51e0118f2b 47 buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 48 buf[2] = 0x08;
howanglam3 0:3b51e0118f2b 49 buf[3] = 0x03;
howanglam3 0:3b51e0118f2b 50 buf[4] = 0x01;
howanglam3 0:3b51e0118f2b 51 buf[5] = GET_LOW_char(time);
howanglam3 0:3b51e0118f2b 52 buf[6] = GET_HIGH_char(time);
howanglam3 0:3b51e0118f2b 53 buf[7] = GET_LOW_char(id);
howanglam3 0:3b51e0118f2b 54 buf[8] = GET_LOW_char(position);
howanglam3 0:3b51e0118f2b 55 buf[9] = GET_HIGH_char(position);
howanglam3 0:3b51e0118f2b 56 serial.write(buf, 10);
howanglam3 0:3b51e0118f2b 57 }
howanglam3 0:3b51e0118f2b 58 void LSCServo::LobotSerialServoMove(vector<uint16_t> id, vector<int16_t> position, uint16_t time) //control list of serial servo move dedicated position
howanglam3 0:3b51e0118f2b 59 {
howanglam3 0:3b51e0118f2b 60 char buf[126];
howanglam3 0:3b51e0118f2b 61 int limit = id.size();
howanglam3 0:3b51e0118f2b 62 int checksum = limit*3 + 6;
howanglam3 0:3b51e0118f2b 63 buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 64 buf[2] = GET_LOW_char(checksum-2);
howanglam3 0:3b51e0118f2b 65 buf[3] = 0x03;
howanglam3 0:3b51e0118f2b 66 buf[4] = GET_LOW_char(limit);
howanglam3 0:3b51e0118f2b 67 buf[5] = GET_LOW_char(time);
howanglam3 0:3b51e0118f2b 68 buf[6] = GET_HIGH_char(time);
howanglam3 0:3b51e0118f2b 69 int index = 7;
howanglam3 0:3b51e0118f2b 70 for(int i = 0; i < limit; i++)
howanglam3 0:3b51e0118f2b 71 {
howanglam3 0:3b51e0118f2b 72 buf[index]=GET_LOW_char(id[i]);
howanglam3 0:3b51e0118f2b 73 index++;
howanglam3 0:3b51e0118f2b 74 if(position[i] < 0)
howanglam3 0:3b51e0118f2b 75 position[i] = 0;
howanglam3 0:3b51e0118f2b 76 if(position[i] > 1000)
howanglam3 0:3b51e0118f2b 77 position[i] = 1000;
howanglam3 0:3b51e0118f2b 78 buf[index]=GET_LOW_char(position[i]);
howanglam3 0:3b51e0118f2b 79 index++;
howanglam3 0:3b51e0118f2b 80 buf[index]=GET_HIGH_char(position[i]);
howanglam3 0:3b51e0118f2b 81 index++;
howanglam3 0:3b51e0118f2b 82 }
howanglam3 0:3b51e0118f2b 83 serial.write(buf,checksum);
howanglam3 0:3b51e0118f2b 84 }
howanglam3 0:3b51e0118f2b 85 void LSCServo::LobotSerialActionGroupRun(uint16_t gpid, uint16_t count) //run action group(action group id, repeat time)
howanglam3 0:3b51e0118f2b 86 {
howanglam3 0:3b51e0118f2b 87 char buf[7];
howanglam3 0:3b51e0118f2b 88 buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 89 buf[2]=0x05;
howanglam3 0:3b51e0118f2b 90 buf[3]=0x06;
howanglam3 0:3b51e0118f2b 91 buf[4]=GET_LOW_char(gpid);
howanglam3 0:3b51e0118f2b 92 buf[5]=GET_LOW_char(count);
howanglam3 0:3b51e0118f2b 93 buf[6]=GET_HIGH_char(count);
howanglam3 0:3b51e0118f2b 94 serial.write(buf,7);
howanglam3 1:1398d9ddd4ef 95
howanglam3 1:1398d9ddd4ef 96 char word[7];
howanglam3 1:1398d9ddd4ef 97 receive_buffer(word, 7);
howanglam3 0:3b51e0118f2b 98 }
howanglam3 0:3b51e0118f2b 99 void LSCServo::LobotSerialActionGroupSpeed(uint16_t gpid, uint16_t speed) //Set Run speed Action Group in XXX%
howanglam3 0:3b51e0118f2b 100 {
howanglam3 0:3b51e0118f2b 101 char buf[7];
howanglam3 0:3b51e0118f2b 102 buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 103 buf[2]=0x05;
howanglam3 0:3b51e0118f2b 104 buf[3]=0x0B;
howanglam3 0:3b51e0118f2b 105 buf[4]=GET_LOW_char(gpid);
howanglam3 0:3b51e0118f2b 106 buf[5]=GET_LOW_char(speed);
howanglam3 0:3b51e0118f2b 107 buf[6]=GET_HIGH_char(speed);
howanglam3 0:3b51e0118f2b 108 serial.write(buf,7);
howanglam3 0:3b51e0118f2b 109 }
howanglam3 0:3b51e0118f2b 110 int LSCServo::LobotSerialGetBatteryVoltage() //get voltage of board in terms of mV
howanglam3 0:3b51e0118f2b 111 {
howanglam3 0:3b51e0118f2b 112 char buf[4];
howanglam3 0:3b51e0118f2b 113 buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 114 buf[2]=0x02;
howanglam3 0:3b51e0118f2b 115 buf[3]=0x0F;
howanglam3 0:3b51e0118f2b 116 serial.write(buf,4);
howanglam3 1:1398d9ddd4ef 117
howanglam3 1:1398d9ddd4ef 118 char word[6];
howanglam3 1:1398d9ddd4ef 119 receive_buffer(word, 6);
howanglam3 1:1398d9ddd4ef 120
howanglam3 1:1398d9ddd4ef 121 int voltage = char_TO_HW(word[5], word[4]);
howanglam3 0:3b51e0118f2b 122 return voltage;
howanglam3 0:3b51e0118f2b 123 }
howanglam3 0:3b51e0118f2b 124 void LSCServo::LobotSerialActionGroupStop() //Stop current action group
howanglam3 0:3b51e0118f2b 125 {
howanglam3 0:3b51e0118f2b 126 char buf[4];
howanglam3 0:3b51e0118f2b 127 buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 128 buf[2]=0x02;
howanglam3 0:3b51e0118f2b 129 buf[3]=0x07;
howanglam3 0:3b51e0118f2b 130 serial.write(buf,4);
howanglam3 0:3b51e0118f2b 131 }