The function library of serial servo controller
Dependents: DR-ArmServoTest ros_button_2021 DR-ArmServoTest
Diff: LSCServo.cpp
- Revision:
- 0:3b51e0118f2b
- Child:
- 1:1398d9ddd4ef
diff -r 000000000000 -r 3b51e0118f2b LSCServo.cpp --- /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