Robocon 2021 / LSCServo

The function library of serial servo controller

Dependents:   DR-ArmServoTest ros_button_2021 DR-ArmServoTest

LSCServo.cpp@2:acaaf1126d76, 2021-06-03 (annotated)

Committer:
stivending
Date:
Thu Jun 03 04:56:29 2021 +0000
Revision:
2:acaaf1126d76
Parent:
1:1398d9ddd4ef 
add moveServos func

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>
stivending 2:acaaf1126d76 7 #include <cstdarg>
howanglam3 0:3b51e0118f2b 8 #include "LSCServo.h"
howanglam3 0:3b51e0118f2b 9
howanglam3 0:3b51e0118f2b 10 #define GET_LOW_char(A) (uint8_t)((A))
howanglam3 0:3b51e0118f2b 11 //Macro function get lower 8 bits of A
howanglam3 0:3b51e0118f2b 12 #define GET_HIGH_char(A) (uint8_t)((A) >> 8)
howanglam3 0:3b51e0118f2b 13 //Macro function get higher 8 bits of A
howanglam3 0:3b51e0118f2b 14 #define char_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B))
howanglam3 0:3b51e0118f2b 15 //put A as higher 8 bits B as lower 8 bits which amalgamated into 16 bits integer
howanglam3 0:3b51e0118f2b 16
howanglam3 0:3b51e0118f2b 17 using namespace std;
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 1:1398d9ddd4ef 27
howanglam3 1:1398d9ddd4ef 28 void LSCServo::receive_buffer(char* destination, int length)
howanglam3 1:1398d9ddd4ef 29 {
howanglam3 1:1398d9ddd4ef 30 int index = 0;
howanglam3 1:1398d9ddd4ef 31 while(index < length)
howanglam3 1:1398d9ddd4ef 32 {
howanglam3 1:1398d9ddd4ef 33 while(!serial.readable());
howanglam3 1:1398d9ddd4ef 34 while(serial.readable())
howanglam3 1:1398d9ddd4ef 35 {
howanglam3 1:1398d9ddd4ef 36 destination[index++] = serial.getc();
howanglam3 1:1398d9ddd4ef 37 }
howanglam3 1:1398d9ddd4ef 38 }
howanglam3 1:1398d9ddd4ef 39 }
howanglam3 1:1398d9ddd4ef 40
howanglam3 0:3b51e0118f2b 41 void LSCServo::LobotSerialoneServoMove(uint16_t id, int16_t position, uint16_t time) //control one serial servo move dedicated position
howanglam3 0:3b51e0118f2b 42 {
howanglam3 0:3b51e0118f2b 43 char buf[10];
howanglam3 0:3b51e0118f2b 44 if(position < 0)
howanglam3 0:3b51e0118f2b 45 position = 0;
howanglam3 0:3b51e0118f2b 46 if(position > 1000)
howanglam3 0:3b51e0118f2b 47 position = 1000;
howanglam3 0:3b51e0118f2b 48 buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 49 buf[2] = 0x08;
howanglam3 0:3b51e0118f2b 50 buf[3] = 0x03;
howanglam3 0:3b51e0118f2b 51 buf[4] = 0x01;
howanglam3 0:3b51e0118f2b 52 buf[5] = GET_LOW_char(time);
howanglam3 0:3b51e0118f2b 53 buf[6] = GET_HIGH_char(time);
howanglam3 0:3b51e0118f2b 54 buf[7] = GET_LOW_char(id);
howanglam3 0:3b51e0118f2b 55 buf[8] = GET_LOW_char(position);
howanglam3 0:3b51e0118f2b 56 buf[9] = GET_HIGH_char(position);
howanglam3 0:3b51e0118f2b 57 serial.write(buf, 10);
howanglam3 0:3b51e0118f2b 58 }
howanglam3 0:3b51e0118f2b 59 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 60 {
howanglam3 0:3b51e0118f2b 61 char buf[126];
howanglam3 0:3b51e0118f2b 62 int limit = id.size();
howanglam3 0:3b51e0118f2b 63 int checksum = limit*3 + 6;
howanglam3 0:3b51e0118f2b 64 buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 65 buf[2] = GET_LOW_char(checksum-2);
howanglam3 0:3b51e0118f2b 66 buf[3] = 0x03;
howanglam3 0:3b51e0118f2b 67 buf[4] = GET_LOW_char(limit);
howanglam3 0:3b51e0118f2b 68 buf[5] = GET_LOW_char(time);
howanglam3 0:3b51e0118f2b 69 buf[6] = GET_HIGH_char(time);
howanglam3 0:3b51e0118f2b 70 int index = 7;
howanglam3 0:3b51e0118f2b 71 for(int i = 0; i < limit; i++)
howanglam3 0:3b51e0118f2b 72 {
howanglam3 0:3b51e0118f2b 73 buf[index]=GET_LOW_char(id[i]);
howanglam3 0:3b51e0118f2b 74 index++;
howanglam3 0:3b51e0118f2b 75 if(position[i] < 0)
howanglam3 0:3b51e0118f2b 76 position[i] = 0;
howanglam3 0:3b51e0118f2b 77 if(position[i] > 1000)
howanglam3 0:3b51e0118f2b 78 position[i] = 1000;
howanglam3 0:3b51e0118f2b 79 buf[index]=GET_LOW_char(position[i]);
howanglam3 0:3b51e0118f2b 80 index++;
howanglam3 0:3b51e0118f2b 81 buf[index]=GET_HIGH_char(position[i]);
howanglam3 0:3b51e0118f2b 82 index++;
howanglam3 0:3b51e0118f2b 83 }
howanglam3 0:3b51e0118f2b 84 serial.write(buf,checksum);
howanglam3 0:3b51e0118f2b 85 }
howanglam3 0:3b51e0118f2b 86 void LSCServo::LobotSerialActionGroupRun(uint16_t gpid, uint16_t count) //run action group(action group id, repeat time)
howanglam3 0:3b51e0118f2b 87 {
howanglam3 0:3b51e0118f2b 88 char buf[7];
howanglam3 0:3b51e0118f2b 89 buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 90 buf[2]=0x05;
howanglam3 0:3b51e0118f2b 91 buf[3]=0x06;
howanglam3 0:3b51e0118f2b 92 buf[4]=GET_LOW_char(gpid);
howanglam3 0:3b51e0118f2b 93 buf[5]=GET_LOW_char(count);
howanglam3 0:3b51e0118f2b 94 buf[6]=GET_HIGH_char(count);
howanglam3 0:3b51e0118f2b 95 serial.write(buf,7);
howanglam3 1:1398d9ddd4ef 96
howanglam3 1:1398d9ddd4ef 97 char word[7];
howanglam3 1:1398d9ddd4ef 98 receive_buffer(word, 7);
howanglam3 0:3b51e0118f2b 99 }
howanglam3 0:3b51e0118f2b 100 void LSCServo::LobotSerialActionGroupSpeed(uint16_t gpid, uint16_t speed) //Set Run speed Action Group in XXX%
howanglam3 0:3b51e0118f2b 101 {
howanglam3 0:3b51e0118f2b 102 char buf[7];
howanglam3 0:3b51e0118f2b 103 buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 104 buf[2]=0x05;
howanglam3 0:3b51e0118f2b 105 buf[3]=0x0B;
howanglam3 0:3b51e0118f2b 106 buf[4]=GET_LOW_char(gpid);
howanglam3 0:3b51e0118f2b 107 buf[5]=GET_LOW_char(speed);
howanglam3 0:3b51e0118f2b 108 buf[6]=GET_HIGH_char(speed);
howanglam3 0:3b51e0118f2b 109 serial.write(buf,7);
howanglam3 0:3b51e0118f2b 110 }
howanglam3 0:3b51e0118f2b 111 int LSCServo::LobotSerialGetBatteryVoltage() //get voltage of board in terms of mV
howanglam3 0:3b51e0118f2b 112 {
howanglam3 0:3b51e0118f2b 113 char buf[4];
howanglam3 0:3b51e0118f2b 114 buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 115 buf[2]=0x02;
howanglam3 0:3b51e0118f2b 116 buf[3]=0x0F;
howanglam3 0:3b51e0118f2b 117 serial.write(buf,4);
howanglam3 1:1398d9ddd4ef 118
howanglam3 1:1398d9ddd4ef 119 char word[6];
howanglam3 1:1398d9ddd4ef 120 receive_buffer(word, 6);
howanglam3 1:1398d9ddd4ef 121
howanglam3 1:1398d9ddd4ef 122 int voltage = char_TO_HW(word[5], word[4]);
howanglam3 0:3b51e0118f2b 123 return voltage;
howanglam3 0:3b51e0118f2b 124 }
howanglam3 0:3b51e0118f2b 125 void LSCServo::LobotSerialActionGroupStop() //Stop current action group
howanglam3 0:3b51e0118f2b 126 {
howanglam3 0:3b51e0118f2b 127 char buf[4];
howanglam3 0:3b51e0118f2b 128 buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
howanglam3 0:3b51e0118f2b 129 buf[2]=0x02;
howanglam3 0:3b51e0118f2b 130 buf[3]=0x07;
howanglam3 0:3b51e0118f2b 131 serial.write(buf,4);
stivending 2:acaaf1126d76 132 }
stivending 2:acaaf1126d76 133
stivending 2:acaaf1126d76 134 void LSCServo::moveServos(uint8_t Num, uint16_t Time, ...)
stivending 2:acaaf1126d76 135 {
stivending 2:acaaf1126d76 136 uint8_t buf[128];
stivending 2:acaaf1126d76 137 va_list arg_ptr;
stivending 2:acaaf1126d76 138 va_start(arg_ptr, Time);
stivending 2:acaaf1126d76 139 if (Num < 1 || Num > 32 || (!(Time > 0))) {
stivending 2:acaaf1126d76 140 return;
stivending 2:acaaf1126d76 141 }
stivending 2:acaaf1126d76 142 buf[0] = LOBOT_SERVO_FRAME_HEADER;
stivending 2:acaaf1126d76 143 buf[1] = LOBOT_SERVO_FRAME_HEADER;
stivending 2:acaaf1126d76 144 buf[2] = Num * 3 + 5;
stivending 2:acaaf1126d76 145 buf[3] = 0x03;
stivending 2:acaaf1126d76 146 buf[4] = Num;
stivending 2:acaaf1126d76 147 buf[5] = GET_LOW_char(Time);
stivending 2:acaaf1126d76 148 buf[6] = GET_HIGH_char(Time);
stivending 2:acaaf1126d76 149 uint8_t index = 7;
stivending 2:acaaf1126d76 150 for (uint8_t i = 0; i < Num; i++) {
stivending 2:acaaf1126d76 151 uint16_t tmp = va_arg(arg_ptr, uint16_t);
stivending 2:acaaf1126d76 152 buf[index++] = GET_LOW_char(tmp);
stivending 2:acaaf1126d76 153
stivending 2:acaaf1126d76 154 uint16_t pos = va_arg(arg_ptr, uint16_t);
stivending 2:acaaf1126d76 155 buf[index++] = GET_LOW_char(pos);
stivending 2:acaaf1126d76 156 buf[index++] = GET_HIGH_char(pos);
stivending 2:acaaf1126d76 157 }
stivending 2:acaaf1126d76 158 va_end(arg_ptr);
stivending 2:acaaf1126d76 159 serial.write(buf, buf[2] + 2);
stivending 2:acaaf1126d76 160 wait_ms(Time);
stivending 2:acaaf1126d76 161 }