There is lots of stuff involved here. Note: there are many up here who are probably a lot better to answer how best to do this setup with PID setup and the like.
The functions like: ReadEncM1, boil down to reading logical registers, which require writing out a command and waiting for a response.
Code:
uint32_t RoboClaw::ReadEncM1(uint8_t address, uint8_t *status,bool *valid){
return Read4_1(address,GETM1ENC,status,valid);
}
Code:
uint32_t RoboClaw::Read4_1(uint8_t address, uint8_t cmd, uint8_t *status, bool *valid){
uint8_t crc;
if(valid)
*valid = false;
uint32_t value=0;
uint8_t trys=MAXRETRY;
int16_t data;
do{
flush();
crc_clear();
write(address);
crc_update(address);
write(cmd);
crc_update(cmd);
data = read(timeout);
crc_update(data);
value=(uint32_t)data<<24;
if(data!=-1){
data = read(timeout);
crc_update(data);
value|=(uint32_t)data<<16;
}
if(data!=-1){
data = read(timeout);
crc_update(data);
value|=(uint32_t)data<<8;
}
if(data!=-1){
data = read(timeout);
crc_update(data);
value|=(uint32_t)data;
}
if(data!=-1){
data = read(timeout);
crc_update(data);
if(status)
*status = data;
}
if(data!=-1){
uint16_t ccrc;
data = read(timeout);
if(data!=-1){
ccrc = data << 8;
data = read(timeout);
if(data!=-1){
ccrc |= data;
if(crc_get()==ccrc){
if(valid)
*valid = true;
return value;
}
}
}
}
}while(trys--);
return false;
}
Couple of issues for you here. First off this is these commands are synchronous. That is you issue the command and then wait for the response.
So it outputs 2 bytes and then I believe try to retrieve 6 byes. Issues with this is:
a) It is synchronous. That is you won't continue, until you receive 6 bytes (4 bytes data 2 bytes CRC) . So assuming everything else you will have to wait for 2 bytes to output and I believe 4 btyes to be returned.
So Serial of 6 bytes. So if no overhead... then maybe about .7ms assuming best case no delays... You do this 6 times so min 4ms
b) wonder how well the timings are as it does multiple reads in a row without delays and the like, so timing may be such as it does not have the next byte available and then does a SerialX.read(), which returns -1, which it thinks is error and starts over...
c) Again having multiple Serial ports is wasted here as each of these calls waits to complete before the next one starts.
One could rewrite the library such that the requests are built into 2 halves. Output the command to read, and then another that sees if enough characters have been received , and extract the data...
So again some of the issues is how the library is setup. You could easily split reading registers like this in to two parts. Send command, and call to see if we have the response. With the 3 serial ports you could cut this time to be one third the time.
But again the drive a motor until encoder returns a value X, inplies by the time we receive it, we may have moved beyond our desired location.
To get around this, the controller may have other controls that may work better for you. Like the ability to set destination position and velocity to get there.
Although the last time I tried this (maybe 3-5 years ago), when you said stop a count X, this is maybe where it started to de-accelerate, so you had to figure out where to set the desitination such that actually stopped at the desired location...
Alternatively sometimes you simply had to guess. That is instead of asking are your there yet, instead you computed that the last command you issued should take time X to complete and after X time has elapsed you assume it completed. At least for RC servos this was more accurate than doing the query and then respond... Not sure if I explained this very well...
Reply With Quote
