bool teo::TechnosoftIpos::getTorqueRaw(int j, double *t) {
//CD_INFO("(%d)\n",j); //-- Too verbose in controlboardwrapper2 stream.
//-- Check index within range
if ( j != 0 ) return false;
//*************************************************************
uint8_t msg_getCurrent[]={0x40,0x7E,0x20,0x00}; // Query current. Ok only 4.
if(! send(0x600, 4, msg_getCurrent) )
{
CD_ERROR("Could not send msg_getCurrent. %s\n", msgToStr(0x600, 4, msg_getCurrent).c_str() );
return false;
}
//CD_SUCCESS("Sent msg_getCurrent. %s\n", msgToStr(0x600, 4, msg_getCurrent).c_str() ); //-- Too verbose in controlboardwrapper2 stream.
//* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
Time::delay(DELAY); // Must delay as it will be from same driver.
//* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
getTorqueReady.wait();
*t = getTorque;
getTorqueReady.post();
//*************************************************************
return true;
}
bool teo::TechnosoftIpos::getEncoderTimedRaw(int j, double *encs, double *time) {
//CD_INFO("(%d)\n",j); //-- Too verbose in controlboardwrapper2 stream.
//-- Check index within range
if ( j != 0 ) return false;
if( ! iEncodersTimedRawExternal ) {
//*************************************************************
uint8_t msg_read[]={0x40,0x64,0x60,0x00,0x00,0x00,0x00,0x00}; // Query position.
if( ! send( 0x600, 8, msg_read) )
{
CD_ERROR("Could not send \"read encoder\". %s\n", msgToStr(0x600, 8, msg_read).c_str() );
return false;
}
//* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
Time::delay(DELAY); // Must delay as it will be from same driver.
//* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
encoderReady.wait();
*encs = encoder;
*time = encoderTimestamp;
encoderReady.post();
//*************************************************************
} else {
iEncodersTimedRawExternal->getEncoderTimedRaw(0,encs,time);
}
return true;
}
bool teo::TechnosoftIpos::setRefTorqueRaw(int j, double t) {
CD_INFO("(%d,%f)\n",j,t);
//-- Check index within range
if ( j!= 0 ) return false;
//*************************************************************
uint8_t msg_ref_torque[]={0x23,0x1C,0x20,0x00,0x00,0x00,0x00,0x00}; // put 23 because it is a target
int sendRefTorque = t * (65520.0/20.0) / (this->tr * this->k); // Page 109 of 263, supposing 10 Ipeak.
//memcpy(msg_ref_torque+4,&sendRefTorque,4); // was +6 not +4, but +6 seems terrible with 4!
memcpy(msg_ref_torque+6,&sendRefTorque,2);
if(! send(0x600, 8, msg_ref_torque) )
{
CD_ERROR("Could not send refTorque. %s\n", msgToStr(0x600, 8, msg_ref_torque).c_str() );
return false;
}
CD_SUCCESS("Sent refTorque. %s\n", msgToStr(0x600, 8, msg_ref_torque).c_str() );
//*************************************************************
return true;
}
bool teo::TechnosoftIpos::velocityMoveRaw(int j, double sp) {
CD_INFO("(%d)\n",j);
//-- Check index within range
if ( j != 0 ) return false;
//*************************************************************
uint8_t msg_vel[]={0x23,0xFF,0x60,0x00,0x00,0x00,0x00,0x00}; // Velocity target
int16_t sendVel = sp * this->tr / 22.5; // Apply tr & convert units to encoder increments
memcpy(msg_vel+6,&sendVel,2);
//float sendVel = sp * this->tr / 22.5; // Apply tr & convert units to encoder increments
//int16_t sendVelFormated = roundf(sendVel * 65536); // 65536 = 2^16
//memcpy(msg_vel+4,&sendVelFormated,4);
if( ! send(0x600, 8, msg_vel)){
CD_ERROR("Sent \"velocity target\". %s\n", msgToStr(0x600, 8, msg_vel).c_str() );
return false;
}
CD_SUCCESS("Sent \"velocity target\". %s\n", msgToStr(0x600, 8, msg_vel).c_str() );
//*************************************************************
return true;
}
char *logToStr2(struct LogEntry *entry) {
char *msg = msgToStr(entry->message);
char *retStr = (char *)malloc( strlen(intToStr(entry->timestamp)) + strlen(intToStr(entry->timeout)) + strlen(intToStr(entry->logID)) + strlen(msg) + 10);
sprintf(retStr, "%d %d %d %s", entry->timestamp, entry->timeout, entry->logID, msg);
return retStr;
}