void CUsbDevice::readServoData(CServo2* servos){
if(connected<1)return;
int i;
char k;
if(sendCtrlMsg(CUSTOM_RQ_LOAD_POS_FROM_I2C, USB_ENDPOINT_IN,0,0,servoDataBuffer)==0){
usleep(10000);
i=sendCtrlMsg(CUSTOM_RQ_GET_POS, USB_ENDPOINT_IN,0,0,servoDataBuffer);
if(i>0){
//printf("received %d {",i);
for (i=0;i<BUFLEN_SERVO_DATA;i++){
//printf("%d,",servoDataBuffer[i]);
if ((servoDataBuffer[i] != servoDataBuffer[0])
|| (servoDataBuffer[i] > USB_HIGHEST_RQ))
k++;
}
//printf("}\n");
if(k==BUFLEN_SERVO_DATA){
for (i=0;i<BUFLEN_SERVO_DATA;i++){
servos[i].setPW( servoDataBuffer[i]);
//printf("got %d:%d\n",i,servoDataBuffer[i]);
//servos[i].setAngle = servos[i].pulsewidthToAngle();
}
}else {
printf("not ready, trying again.\n");
}
}
}else printf("readServoData: LOAD_POS_FROM_I2C failed\n");
}
void CUsbDevice::readServoData(){
if(connected<1) return;
int i;
char k;
if (sendCtrlMsg(CUSTOM_RQ_LOAD_POS_FROM_I2C, USB_ENDPOINT_IN,0,0,servoDataBuffer)==0){;
i=sendCtrlMsg(CUSTOM_RQ_GET_POS, USB_ENDPOINT_IN,0,0,servoDataBuffer);
if(i>0){
printf("received %d {",i);
for (i=0;i<BUFLEN_SERVO_DATA;i++){
printf("%d,",servoDataBuffer[i]);
}
printf("}\n");
}
//store in servo's
}else printf("readServoData: device was not ready\n");
}
void CUsbDevice::sendServoData(CServo2 *servos){
if(connected<1) return;
int i;
for (i=0;i<BUFLEN_SERVO_DATA;i++){
servoDataBuffer[i] = servos[i].getPW();
}
i = sendCtrlMsg(CUSTOM_RQ_SET_DATA, USB_ENDPOINT_OUT, 0,0,servoDataBuffer);
//printf("sendServoData: %d send\n",i);
}
int CUsbDevice::getData(){
if(connected<1) return 0;
char i;
sendCtrlMsg(CUSTOM_RQ_GET_DATA, USB_ENDPOINT_IN,0,0,PSControllerDataBuffer);
return 1;
// printf("Right X = %d\n",PSControllerDataBuffer[5]);
// printf("Right Y = %d\n",PSControllerDataBuffer[6]);
// printf("Left X = %d\n",PSControllerDataBuffer[7]);
// printf("Left Y = %d\n",PSControllerDataBuffer[8]);
// printf("data received %d {",i);
// for (i=0;i<BUFLEN_SERVO_DATA;i++){
// printf("0x%hX,",PSControllerDataBuffer[i]);
// }
// printf("}\n");
}
void CUsbDevice::sendServoData(){
if(connected<1) return;
int i;
i = sendCtrlMsg(CUSTOM_RQ_SET_DATA, USB_ENDPOINT_OUT, 0,0,servoDataBuffer);
//printf("sendServoData: %d send\n",i);
}
bool Job::sendCtrlMsg2Parent( CtrlMsg& msg, int fromChild )
{
Debug( Job, "fromChild=%d\n", fromChild );
return sendCtrlMsg( msg, m_localNidMap[ 0 ] );
}
bool Job::sendCtrlMsg2Child( CtrlMsg& msg, int toChild )
{
Debug( Job, "toChild=%d\n",toChild );
return sendCtrlMsg( msg, m_localNidMap[ toChild + 1 ] );
}
