The function library of serial servo controller
Dependents: DR-ArmServoTest ros_button_2021 DR-ArmServoTest
LSCServo.cpp
- Committer:
- howanglam3
- Date:
- 2021-05-19
- Revision:
- 0:3b51e0118f2b
- Child:
- 1:1398d9ddd4ef
File content as of revision 0:3b51e0118f2b:
//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); }