Robocon 2021 / LSCServo

The function library of serial servo controller

Dependents:   DR-ArmServoTest ros_button_2021 DR-ArmServoTest

LSCServo.cpp@0:3b51e0118f2b, 2021-05-19 (annotated)

Committer:
howanglam3
Date:
Wed May 19 09:51:20 2021 +0000
Revision:
0:3b51e0118f2b
Child:
1:1398d9ddd4ef
Library function for the LSC serial servo controller

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
howanglam3 0:3b51e0118f2b 19 //constructor
howanglam3 0:3b51e0118f2b 20 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 21 {
howanglam3 0:3b51e0118f2b 22 }
howanglam3 0:3b51e0118f2b 23 //destructor
howanglam3 0:3b51e0118f2b 24 LSCServo::~LSCServo()
howanglam3 0:3b51e0118f2b 25 {
howanglam3 0:3b51e0118f2b 26 }
howanglam3 0:3b51e0118f2b 27 void LSCServo::LobotSerialoneServoMove(uint16_t id, int16_t position, uint16_t time) //control one serial servo move dedicated position
howanglam3 0:3b51e0118f2b 28 {
howanglam3 0:3b51e0118f2b 29 char buf[10];
howanglam3 0:3b51e0118f2b 30 if(position < 0)
howanglam3 0:3b51e0118f2b 31 position = 0;
howanglam3 0:3b51e0118f2b 32 if(position > 1000)
howanglam3 0:3b51e0118f2b 33 position = 1000;
howanglam3 0:3b51e0118f2b 34 buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 35 buf[2] = 0x08;
howanglam3 0:3b51e0118f2b 36 buf[3] = 0x03;
howanglam3 0:3b51e0118f2b 37 buf[4] = 0x01;
howanglam3 0:3b51e0118f2b 38 buf[5] = GET_LOW_char(time);
howanglam3 0:3b51e0118f2b 39 buf[6] = GET_HIGH_char(time);
howanglam3 0:3b51e0118f2b 40 buf[7] = GET_LOW_char(id);
howanglam3 0:3b51e0118f2b 41 buf[8] = GET_LOW_char(position);
howanglam3 0:3b51e0118f2b 42 buf[9] = GET_HIGH_char(position);
howanglam3 0:3b51e0118f2b 43 serial.write(buf, 10);
howanglam3 0:3b51e0118f2b 44 }
howanglam3 0:3b51e0118f2b 45 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 46 {
howanglam3 0:3b51e0118f2b 47 char buf[126];
howanglam3 0:3b51e0118f2b 48 int limit = id.size();
howanglam3 0:3b51e0118f2b 49 int checksum = limit*3 + 6;
howanglam3 0:3b51e0118f2b 50 buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 51 buf[2] = GET_LOW_char(checksum-2);
howanglam3 0:3b51e0118f2b 52 buf[3] = 0x03;
howanglam3 0:3b51e0118f2b 53 buf[4] = GET_LOW_char(limit);
howanglam3 0:3b51e0118f2b 54 buf[5] = GET_LOW_char(time);
howanglam3 0:3b51e0118f2b 55 buf[6] = GET_HIGH_char(time);
howanglam3 0:3b51e0118f2b 56 int index = 7;
howanglam3 0:3b51e0118f2b 57 for(int i = 0; i < limit; i++)
howanglam3 0:3b51e0118f2b 58 {
howanglam3 0:3b51e0118f2b 59 buf[index]=GET_LOW_char(id[i]);
howanglam3 0:3b51e0118f2b 60 index++;
howanglam3 0:3b51e0118f2b 61 if(position[i] < 0)
howanglam3 0:3b51e0118f2b 62 position[i] = 0;
howanglam3 0:3b51e0118f2b 63 if(position[i] > 1000)
howanglam3 0:3b51e0118f2b 64 position[i] = 1000;
howanglam3 0:3b51e0118f2b 65 buf[index]=GET_LOW_char(position[i]);
howanglam3 0:3b51e0118f2b 66 index++;
howanglam3 0:3b51e0118f2b 67 buf[index]=GET_HIGH_char(position[i]);
howanglam3 0:3b51e0118f2b 68 index++;
howanglam3 0:3b51e0118f2b 69 }
howanglam3 0:3b51e0118f2b 70 serial.write(buf,checksum);
howanglam3 0:3b51e0118f2b 71 }
howanglam3 0:3b51e0118f2b 72 void LSCServo::LobotSerialActionGroupRun(uint16_t gpid, uint16_t count) //run action group(action group id, repeat time)
howanglam3 0:3b51e0118f2b 73 {
howanglam3 0:3b51e0118f2b 74 char buf[7];
howanglam3 0:3b51e0118f2b 75 buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 76 buf[2]=0x05;
howanglam3 0:3b51e0118f2b 77 buf[3]=0x06;
howanglam3 0:3b51e0118f2b 78 buf[4]=GET_LOW_char(gpid);
howanglam3 0:3b51e0118f2b 79 buf[5]=GET_LOW_char(count);
howanglam3 0:3b51e0118f2b 80 buf[6]=GET_HIGH_char(count);
howanglam3 0:3b51e0118f2b 81 serial.write(buf,7);
howanglam3 0:3b51e0118f2b 82 }
howanglam3 0:3b51e0118f2b 83 void LSCServo::LobotSerialActionGroupSpeed(uint16_t gpid, uint16_t speed) //Set Run speed Action Group in XXX%
howanglam3 0:3b51e0118f2b 84 {
howanglam3 0:3b51e0118f2b 85 char buf[7];
howanglam3 0:3b51e0118f2b 86 buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 87 buf[2]=0x05;
howanglam3 0:3b51e0118f2b 88 buf[3]=0x0B;
howanglam3 0:3b51e0118f2b 89 buf[4]=GET_LOW_char(gpid);
howanglam3 0:3b51e0118f2b 90 buf[5]=GET_LOW_char(speed);
howanglam3 0:3b51e0118f2b 91 buf[6]=GET_HIGH_char(speed);
howanglam3 0:3b51e0118f2b 92 serial.write(buf,7);
howanglam3 0:3b51e0118f2b 93 }
howanglam3 0:3b51e0118f2b 94 int LSCServo::LobotSerialGetBatteryVoltage() //get voltage of board in terms of mV
howanglam3 0:3b51e0118f2b 95 {
howanglam3 0:3b51e0118f2b 96 char buf[4];
howanglam3 0:3b51e0118f2b 97 buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 98 buf[2]=0x02;
howanglam3 0:3b51e0118f2b 99 buf[3]=0x0F;
howanglam3 0:3b51e0118f2b 100 serial.write(buf,4);
howanglam3 0:3b51e0118f2b 101 while(serial.readable()==0)
howanglam3 0:3b51e0118f2b 102 {
howanglam3 0:3b51e0118f2b 103 ;
howanglam3 0:3b51e0118f2b 104 }
howanglam3 0:3b51e0118f2b 105 MyBuffer<char> rx = serial.getc();
howanglam3 0:3b51e0118f2b 106 int limit = rx.getSize();
howanglam3 0:3b51e0118f2b 107 char* word;
howanglam3 0:3b51e0118f2b 108 for(int i = 0; i < limit; i++)
howanglam3 0:3b51e0118f2b 109 {
howanglam3 0:3b51e0118f2b 110 *(word + i) = rx;
howanglam3 0:3b51e0118f2b 111 }
howanglam3 0:3b51e0118f2b 112 int voltage = char_TO_HW(*(word + 5), *(word + 4));
howanglam3 0:3b51e0118f2b 113 return voltage;
howanglam3 0:3b51e0118f2b 114 }
howanglam3 0:3b51e0118f2b 115 void LSCServo::LobotSerialActionGroupStop() //Stop current action group
howanglam3 0:3b51e0118f2b 116 {
howanglam3 0:3b51e0118f2b 117 char buf[4];
howanglam3 0:3b51e0118f2b 118 buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 119 buf[2]=0x02;
howanglam3 0:3b51e0118f2b 120 buf[3]=0x07;
howanglam3 0:3b51e0118f2b 121 serial.write(buf,4);
howanglam3 0:3b51e0118f2b 122 }