int FaulHaberApi::setVelocity(int id, int vel)
{
msg = buildMessageData(WriteObjectId,id,8,0x23,0x6084,0x00,vel);
int status = writeWaitReadMessage(&msg);
//qDebug()<<"set velocity"<<status;
return status;
}
int FaulHaberApi::syncGetPosition(int node_count,int* nodeIds,int* positions)
{
for(int count=0;count<node_count;count++)
{
//msgs[count] = buildMessageData(WriteObjectId,nodeIds[count],8,0x40,0x0000,0x00,0x0000000000);
msgs[count] = buildMessageData(0x300,nodeIds[count],8,0x40,0x0000,0x00,0x0000000000);
}
int status = multiWriteWaitReadMessage(msgs, node_count);
int pos = 0;
for(int count=0;count<node_count;count++)
{
pos = 0;
while ((int64_t)nodeIds[pos] != (int64_t)msgs[count].Id %0x10)
{
pos++;
if(pos > node_count +1)
return -1;
}
positions[pos] = readIntegerResponse(msgs[count]);
}
return status;
}
void FaulHaberApi::resetBus()
{
VSCAN_MSG msg = buildMessageData(0x00,0x00,2,0x01,0x0000,0x00,0x00000000);
writeWaitReadMessage(&msg);
writeWaitReadMessage(&msg);
writeWaitReadMessage(&msg);
sleep(5);
}
int FaulHaberApi::syncSetVelocity(int node_count,int* nodeIds,int* velocities)
{
for(int count=0;count<node_count;count++)
{
msgs[count] = buildMessageData(WriteObjectId,nodeIds[count],8,0x23,0x6084,0x00,velocities[count]);//set position
}
int status = multiWriteWaitReadMessage(msgs, node_count);
return status;
}
void FaulHaberApi::Init_Node(int id)
{
//tres tramas iniciales
msg = buildMessageData(CanOpenId,id,2,0x01,0x0000,0x00,0x00000000);
writeWaitReadMessage(&msg);
writeWaitReadMessage(&msg);
writeWaitReadMessage(&msg);
writeWaitReadMessage(&msg);
writeWaitReadMessage(&msg);
writeWaitReadMessage(&msg);
int result = 0;
//switch on disable
msg = buildMessageData(WriteObjectId,id,8,0x2B,0x6040,0x00,0x00000000);
result = writeWaitReadMessage(&msg);
if(id == 3)
sleep(1);
//ready to switch on
msg = buildMessageData(WriteObjectId,id,8,0x2B,0x6040,0x00,0x00000006);
result += writeWaitReadMessage(&msg);
//switch on voltage
msg = buildMessageData(WriteObjectId,id,8,0x2B,0x6040,0x00,0x00000007);
result += writeWaitReadMessage(&msg);
//operation enabled
msg = buildMessageData(WriteObjectId,id,8,0x2B,0x6040,0x00,0x0000000F);
result += writeWaitReadMessage(&msg);
/* if (result != 4)
qFatal("error setting up node");*/
//2B => write??
//2F => read??
}
int FaulHaberApi::getPositionExternalEncoder(int id)
{
//qDebug()<<"read position external encoder"<<id;
msg = buildMessageData(0x300,id,8,0xb2,0x0005,0x00,0x00000000);
int a=writeWaitReadMessage(&msg);
//printf("output %#x, %#x, %#x, %#x, %#x, %#x, %#x, %#x, %#x, %#x \n ", msg.Id,msg.Size, msg.Data[0], msg.Data[1], msg.Data[2], msg.Data[3], msg.Data[4], msg.Data[5], msg.Data[6], msg.Data[7], msg.Timestamp );
if(a == 1)
return readIntegerResponse(msg);
else
return -1;
}
int FaulHaberApi::getPosition(int id)
{
//qDebug()<<"read position"<<id;
//msg = buildMessageData(WriteObjectId,id,8,0x40,0x0000,0x00,0x00000000);
msg = buildMessageData(0x300,id,8,0x40,0x0000,0x00,0x00000000);
int a=writeWaitReadMessage(&msg);
if(a == 1)
return readIntegerResponse(msg);
else
{
printf("output %lx, %x, %x, %x, %x, %x, %x, %x, %x, %x %d\n ", msg.Id, msg.Size, msg.Data[0], msg.Data[1], msg.Data[2], msg.Data[3], msg.Data[4], msg.Data[5], msg.Data[6], msg.Data[7], msg.Timestamp);
return -1;
}
}
int FaulHaberApi::disablePower(int id)
{
msg = buildMessageData(WriteObjectId,id,8,0x2B,0x3004, 0x00, 0x00000001);//no bien construida
int status = writeWaitReadMessage(&msg);
return status;
}
void FaulHaberApi::enableCommandMode(int id)
{
msg = buildMessageData(WriteObjectId,id,8,0x2F,0x6060,0x00,0x000000FF);
/*int status = */writeWaitReadMessage(&msg);
// qDebug()<<"enable mode result "<<status;
}
void FaulHaberApi::setDeceleration(int id, int deceleration)
{
msg = buildMessageData(WriteObjectId,id,8,0x23,0x6084,0x00,deceleration);
/*int status = */writeWaitReadMessage(&msg);
// qDebug()<<"set deceleration"<<status;
}
void FaulHaberApi::enableControlPositionMode(int id)
{
msg = buildMessageData(WriteObjectId,id,8,0x2F,0x6060,0x00,0x00000001);
int status = writeWaitReadMessage(&msg);
// qDebug()<<"enable mode result:"<<status;
}
void FaulHaberApi::setAceleration(int id, int aceleration)
{
msg = buildMessageData(WriteObjectId,id,8,0x23,0x6083,0x00,aceleration);
int status = writeWaitReadMessage(&msg);
// qDebug()<<"set aceleration"<<status;
}
