//rutine for wake up goo!
void DynamicGoo::neighborsFound(){
//set flag false
sleeping=false;
counter=qrand()%10;
//call rutine for move to next goo
moveToTarget();
}
/**
* \brief Entering the robot into the "Attachement mode"
* \detailed In this mode, the robot will look for a source of high heat and will follow it. If the robot doesn't find a source
* after a period of time, it goes into a panic mode, which is turning around itself forever until it detects a heat source and follows it.
* \param pvParameters - pointer to parameters
*/
static void attachmentMode(void * pvParameters) {
TickType_t xLastWakeTime = xTaskGetTickCount();
uint8_t distance = 0, panicModeCounter = 0, panicModeEnabledCount = 250;
uint16_t currentPulseWidthTicks = INITIAL_PULSE_WIDTH_TICKS;
// Initialize as search mode
ATTACHMENT_MODE = SEARCH_MODE;
// Head needs to be facing forward
motion_servo_start(MOTION_SERVO_CENTER);
rotateHead(¤tPulseWidthTicks);
while (1) {
// If we have new temperature data
if(hasNewValues == 1){
hasNewValues = 0;
// the highest temperature sensed is greater than the trigger temperature
if ((uint8_t) TRIGGER_TEMPERATURE < temperatures[indexOfHighestTemperature]) {
// We are tracking a heat source!
ATTACHMENT_MODE = TRACKING_MODE;
// reset panic mode counter
panicModeCounter = 0;
// get the distance to the heat source
distance = getDistance();
// if the highest heat source is not in the center
if (indexOfHighestTemperature != 4 || indexOfHighestTemperature != 5) {
// turn towards the heat source
turnToAngle(45 - 10 * indexOfHighestTemperature);
}
// get as close to target as possible
moveToTarget(distance);
// no temperature is higher than the trigger temperature
// increment the panic mode counter and check if we have reached the panic mode enabled count
} else if (panicModeEnabledCount <= panicModeCounter) {
ATTACHMENT_MODE = PANIC_MODE;
setMovementType(ROTATE_RIGHT_MOVEMENT);
} else {
++panicModeCounter;
// Stop movement because we're no longer tracking a heat source
setMovementType(STOP_MOVEMENT);
// No longer in tracking or panic mode
ATTACHMENT_MODE = SEARCH_MODE;
}
}
vTaskDelayUntil(&xLastWakeTime, ((30000 / panicModeEnabledCount) / portTICK_PERIOD_MS));
}
}
void DynamicGoo::update(){
if (isSelected() && !isFalling()) {
body->SetLinearVelocity(b2Vec2(0,0));
return;
}
if (!isSleeping()){
if (hasJoint()){
body->SetAngularVelocity(0.0);
}
moveToTarget();
}
else
emit this->checkForNeighbors(getPPosition());
}
void RandomWaypointNode::think(SimTime *simTime)
{
//Se ho sbattuto contro un muro calcolo nuva direzione
if(collided==true)
{
//pause=true;
//Calcolo il nuovo vettore velocitÃ
v.x=(float)((((float)rand()/(float)(RAND_MAX))*(vMax-vMin))+vMin)
*(rand()%2<1?(-1):(1));//Segno
v.y=(float)((((float)rand()/(float)(RAND_MAX))*(vMax-vMin))+vMin)
*(rand()%2<1?(-1):(1));//Segno
//Rapporto la velocità all'intervallo di tempo
v.x=v.x*simTime->getDT();
v.y=v.y*simTime->getDT();
}
else
{
//System.out.println(toString());
if(pause==true)
{
if(localTime<pauseTime)
{v.x=0; v.y=0;}
else
{
pause=false;
//Calcolo un nuovo target
generateTarget();
moveToTarget(simTime);//Mi muovo verso il target
}
}
else
{moveToTarget(simTime);} //Mi muovo verso il target
}
//Incremento il tempo locale
localTime+=simTime->getDT();
}//Fine think
task main() {
pidSetup();
StartTask(checkObstacles);
wait1Msec(500);
while(true){
wait1Msec(400);
if(handleObstacleLeft){
StopTask(obstacleHandler);
StartTask(obstacleHandler);
} else if(handleObstacleRight){
StopTask(obstacleHandler);
StartTask(obstacleHandler);
} else if(!passingByObstacle){
moveToTarget();
}
}
}
void DynamicGoo::update() {
QList <int > toRemove;
for (int i=0; i<sources.length(); i++) {
if (!sSystem->sourceStatus(sources[i].first)) {
sSystem->deleteSource(sources[i]);
toRemove.push_back(i);
}
}
for (int i=0; i<toRemove.length(); i++)
sources.removeAt(toRemove[i]);
if (isSelected() && !isFalling()) {
body->SetLinearVelocity(b2Vec2(0,0));
return;
}
if (!isSleeping()) {
if (hasJoint()) {
body->SetAngularVelocity(0.0);
}
moveToTarget();
}
else
emit this->checkForNeighbors(getPPosition());
}
void MainWindow::AutoSequence0( void )
{
QString CMD;
QString FUNC;
QString VAL, VAL2;
QStringList LINE;
bool progress = true;
if ( AutoSequenceShouldBeLocked() )
return;
while ( ASCMDi < ASCMDs.count() ) {
LINE = ASCMDs[ ASCMDi ];
// 後ろから前へ置換する
// 2つ目以降は $aa:$bb の aa も bb もそれ以降も全部置換する
for ( int i = LINE.count()-1; i > 0; i-- ) {
int p;
while( ( p = LINE[i].lastIndexOf( '$', -1 ) ) >= 0 ) {
QString theVal = LINE[i].mid( p + 1 );
LINE[i] = LINE[i].left( p ) + ASVals[ theVal ];
}
}
// 変数置換
if ( LINE[0][0] == '$' ) { // 最初の項目は $aa:$bb の bb は置換するけど aa はしない
int p;
while( ( p = LINE[0].lastIndexOf( '$', -1 ) ) > 0 ) {
QString theVal = LINE[0].mid( p + 1 );
LINE[0] = LINE[0].left( p ) + ASVals[ theVal ];
}
}
//
CMD = FUNC = VAL = VAL2 = "";
if ( LINE.count() > 0 ) CMD = LINE[0];
if ( LINE.count() > 1 ) FUNC = LINE[1];
if ( LINE.count() > 2 ) VAL = LINE[2];
if ( LINE.count() > 3 ) VAL2 = LINE[3];
if ( CMD[0] == '$' ) {
ASVals[ CMD.mid(1) ] = FUNC;
} else if ( CMD == "ARRAY" ) {
for ( int i = 2; i < LINE.count(); i++ ) {
ASVals[ FUNC+":"+QString::number(i-2) ] = LINE[i];
}
} else if ( CMD == "INC" ) {
ASVals[ FUNC ] = QString::number( ASVals[ FUNC ].toInt() + 1 );
} else if ( CMD == "ADD" ) {
ASVals[ FUNC ] = QString::number( ASVals[ FUNC ].toInt() + VAL.toInt() );
} else if ( CMD == "SADD" ) {
ASVals[ FUNC ] = ASVals[ FUNC ] + VAL;
} else if ( CMD == "SHOW" ) {
qDebug() << "Show " << LINE;
} else if ( CMD == "MONOCH" ) {
if ( FUNC == "ENERGY" ) {
CheckMUnits.add( MMainTh, true, true );
MoveCurThPosKeV( VAL.toDouble() );
}
} else if ( CMD == "SCHANGER" ) {
if ( FUNC == "SELECT" ) { // SCHANGER SELECT
int i;
for ( i = 0; i < SChangers.count(); i++ ) {
if ( SChangers[i]->id() == VAL ) {
ASSChanger = SChangers[i];
break;
}
}
if ( i >= SChangers.count() ) {
qDebug() << "Can not find the selected Special Changer " << VAL;
AutoSequenceEnd();
}
} else if ( FUNC == "SETDXDY" ) {
if ( VAL == "ASIS" ) {
ASSCDx = AttDx->text().toDouble();
ASSCDy = AttDy->text().toDouble();
} else {
ASSCDx = VAL.toDouble();
ASSCDy = VAL2.toDouble();
}
} else if ( FUNC == "MOVE" ) { // SCHANGER MOVE
int i;
for ( i = 0; i < ASSChanger->specNames(); i++ ) {
if ( ASSChanger->specName( i )->attrib( 0 ) == VAL ) {
CheckMUnits.add( ASSChanger->baseChangerP()->unit1(), true, true );
CheckMUnits.add( ASSChanger->baseChangerP()->unit2(), true, true );
moveToTarget( ASSChanger->baseChangerP(),
ASSChanger->specName( i )->position(),
ASSCDx, ASSCDy );
break;
}
}
if ( i >= ASSChanger->specNames() ) {
qDebug() << "Can not find the selected Special Changer position" << VAL;
}
}
} else if ( CMD == "MEASURE" ) {
if ( FUNC == "CLEAR" ) {
ASMUnits.clearUnits();
} else if ( FUNC == "SENS" ) {
int i;
for ( i = 0; i < ASensors.count(); i++ ) {
if ( ASensors[i]->uid() == VAL ) {
ASMUnits.addUnit( ASensors[i] );
break;
}
}
if ( i >= ASensors.count() ) {
qDebug() << "Can not find the selected Sensor" << VAL;
}
} else if ( FUNC == "DTIME" ) {
ASMUnits.setDwellTimes( VAL.toDouble() );
ASMUnits.setDwellTime();
} else if ( FUNC == "MEASURE" ) {
progress = false;
switch( ASCMDii ) {
case 0:
ASMUnits.clearStage();
ASCMDii++;
break;
case 1:
if ( !ASMUnits.init() ) {
ASMUnits.clearStage();
ASCMDii++;
}
break;
case 2:
if (( !ASMUnits.isParent() )||( ! ASMUnits.getValue0() )) {
ASMUnits.clearStage();
ASCMDii++;
}
break;
case 3:
if ( ! ASMUnits.getValue() ) {
ASCMDii = 0; // ASCMDii で回るサブループの最後では必ず ASCMDii = 0;
ASMUnits.clearStage(); // これは不要のはずだけど念の為
progress = true;
}
break;
}
} else if ( FUNC == "READ" ) {
ASMUnits.readValue( ASMeasVals, ASMeasCPSs, false ); // true : correct dark
}
} else if ( CMD == "FILE" ) {
VAL.remove( QChar( '"' ) );
QIODevice::OpenMode mode;
QString Item;
QFile f( VAL );
if ( ( FUNC == "WRITE" )||( FUNC == "APPEND" ) ) {
if ( FUNC == "WRITE" ) {
mode = QIODevice::WriteOnly | QIODevice::Text;
} else {
mode = QIODevice::Append | QIODevice::Text;
}
if ( f.open( mode ) ) {
QTextStream out( &f );
for ( int i = 3; i < LINE.count(); i++ ) {
Item = LINE[i].replace( QChar( '~' ), QChar( ' ' ) );
Item.remove( QChar( '"' ) );
if ( Item == "@DATE" ) {
out << QDate::currentDate().toString( "yyyy/MM/dd" );
} else if ( Item == "@TIME" ) {
out << QTime::currentTime().toString( "hh:mm:ss" );
} else if ( Item == "@RING" ) {
if ( SLS != NULL ) {
out << SLS->value();
}
} else if ( Item == "@MEASURED" ) {
for ( int i = 0; i < ASMUnits.count(); i++ ) {
out << ASMeasVals[i] << "\t";
}
} else {
out << Item;
}
}
out << endl;
f.close();
} else {
qDebug() << "Cannot open file " << VAL << FUNC;
}
} else {
qDebug() << "Wrong file mode " << VAL << FUNC;
}
} else if ( CMD == "MEASSSD" ) {
if ( ( SFluo != NULL )||( ( VAL != "REAL" )&&( VAL != "LIVE" ) ) ) {
if ( FUNC == "MEAS" ) {
progress = false;
switch( ASCMDii ) {
case 0:
ASMUnits.clearUnits();
ASMUnits.addUnit( SFluo );
SFluo->initLocalStage();
ASCMDii++;
break;
case 1:
qDebug() << "set preset type";
SFluo->setSSDPresetType( VAL );
ASCMDii++;
break;
case 2:
SFluo->SetTime( VAL2.toDouble() );
ASCMDii++;
break;
case 3:
qDebug() << "try to init sensor and go";
if ( SFluo->InitSensor() == false ) { // true :: initializing
SFluo->setIsBusy( true );
SFluo->RunStart();
ASCMDii++;
}
break;
case 4:
SFluo->GetMCAs();
ASCMDii = 0;
progress = true;
break;
}
} else if ( FUNC == "MODE" ) {
SFluo->setSSDPresetType( VAL );
CheckMUnits.add( SFluo, true, true );
} else if ( FUNC == "RECORD" ) {
VAL.remove( QChar( '"' ) );
aMCASet *set = new aMCASet;
SaveMCADataOnMem( set );
set->save( VAL, "measured by auto Sequence" );
// saveMCAData0( VAL, set );
delete set;
}
} else {
if ( SFluo == NULL ) qDebug() << "MEASSSD: SSD is not available";
if (( VAL != "REAL" )&&( VAL != "LIVE" ))
qDebug() << "MEASSSD: only REAL or LIVE is availabe";
}
} else if ( CMD == "LABEL" ) {
// ラベルの処理はここではしない
} else if ( CMD == "GOTO" ) {
ASCMDi = ASLabels[ FUNC ];
} else if ( CMD == "IFEQ" ) {
if ( FUNC != VAL )
ASCMDi++;
} else if ( CMD == "IFNEQ" ) {
if ( FUNC == VAL )
ASCMDi++;
} else if ( CMD == "END" ) {
break; // goto AutoSequenceStop
} else {
qDebug() << "No CMD [" << CMD << "]";
}
if ( progress )
ASCMDi++;
return;
}
AutoSequenceEnd();
}
