bool checkForDog(int angle){
if (readProximity()) return true;
turnRightUntil(angle);
if (readProximity()) return true;
turnLeftUntil(angle * 2);
if (readProximity()) return true;
turnRightUntil(angle);
return false;
}
void iqrcommon::moduleEPuck::update(){
// cout << "moduleEPuck::update()" << endl;
iCounter++;
#ifdef IQRMODULE
qmutexThread->lock(); //new
StateArray &clsStateArrayProxialOutput = clsStateVariableProxialOutput->getStateArray();
StateArray &clsStateArrayAmbientOutput = clsStateVariableAmbientOutput->getStateArray();
StateArray &clsStateArrayMotorInput = stateVariableMotorIn->getTarget()->getStateArray();
#else
StateArray clsStateArrayMotorInput(16);
clsStateArrayMotorInput[0][2] = rand()/RAND_MAX;
clsStateArrayMotorInput[0][3] = rand()/RAND_MAX;;
clsStateArrayMotorInput[0][14] = rand()/RAND_MAX;
clsStateArrayMotorInput[0][15] = rand()/RAND_MAX;
StateArray clsStateArrayProxialOutput(8);
StateArray clsStateArrayAmbientOutput(8);
#endif
if(bShowOutput){
clsEPuckDisp->setAmbient(clsStateArrayAmbientOutput[0]);
clsEPuckDisp->setProximity(clsStateArrayProxialOutput[0]);
}
readProximity(clsStateArrayProxialOutput);
readAmbient(clsStateArrayAmbientOutput);
setSpeed(clsStateArrayMotorInput);
// cout << "Proximity: ";
// printStateArray(clsStateArrayProxialOutput);
// cout << "Ambient: ";
// printStateArray(clsStateArrayAmbientOutput);
// cout << "Motor: ";
// printStateArray(clsStateArrayMotorInasput);
qmutexThread->unlock(); //new
#ifndef IQRMODULE
// usleep(300000);
sleep(1);
#endif
usleep(100000);
};
bool moveUntilProximity(int distance, int speed) {
setMotorSpeed(speed,speed, FORWARD, FORWARD);
int prevVal = checkOdometry();
int startDistance = distance;
while (distance > 0){
if(readProximity())
return true;
int val = checkOdometry();
if (prevVal != val){
distance--;
prevVal = val;
}
}
return false;
}
