Ejemplo n.º 1
0
// ----------------------------------------------------------------------------
// Strategy2005CL::checkEndEvents
// ----------------------------------------------------------------------------
// Execute les commandes en cas d'arret d'urgence, de fin de match
// Retourne false si la strategy est terminee et qu'il faut retourner au menu
// endEvt=true si l'evenement a ete catche
// ----------------------------------------------------------------------------
bool Strategy2005CL::checkEndEvents()
{
    if (matchIsFinished_) {
        return true;
    }
#ifdef STOP_ON_EMERGENCY_STOP
    if (Events->isInWaitResult(EVENTS_EMERGENCY_STOP)
	|| Events->check(EVENTS_EMERGENCY_STOP)) {
        emergencyStop();
        return true;
    } else 
#endif
    if (Events->isInWaitResult(EVENTS_GAME_OVER)
	       || Events->check(EVENTS_GAME_OVER)) {
        gameOver();
        return true;
    } else if (Events->isInWaitResult(EVENTS_TIMER_ALERT)
	       || Events->check(EVENTS_TIMER_ALERT)) {
        return timerAlert();
    } else if (Events->isInWaitResult(EVENTS_USER_ABORT)
	       || Events->check(EVENTS_USER_ABORT)) {
        return true;
    }
    return false;
}
Ejemplo n.º 2
0
void eaps8000UsbUICharWindow::connectUiElements()
{
    connect( _ui->btn_emergencyStop, SIGNAL( clicked() ), this,
             SLOT( emergencyStop() ) );
    connect( _ui->cob_setValue, SIGNAL( currentTextChanged( QString ) ), this,
             SLOT( setValueSelectionChanged() ) );
    connect( _ui->cob_unit, SIGNAL( currentTextChanged( QString ) ),
             this, SLOT( updateUnitRange() ) );

    connect( _ui->chb_calcValues, SIGNAL( stateChanged( int ) ), this,
             SLOT( fixStepSizeChanged() ) );

    connect( _ui->dsb_fromValue, SIGNAL( valueChanged( double ) ), this,
             SLOT( calculateRemainingTicks() ) );
    connect( _ui->dsb_toValue, SIGNAL( valueChanged( double ) ), this,
             SLOT( calculateRemainingTicks() ) );
    connect( _ui->dsb_stepSize, SIGNAL( valueChanged( double ) ), this,
             SLOT( calculateRemainingTicks() ) );
    connect( _ui->spb_repeat, SIGNAL( valueChanged( int ) ), this,
             SLOT( calculateRemainingTicks() ) );
    connect( _ui->chb_loop, SIGNAL( stateChanged( int ) ), this,
             SLOT( calculateRemainingTicks() ) );

    connect( _ui->btn_connect, SIGNAL( clicked() ), this,
             SLOT( connectivityButtonPressed() ) );
    connect( _ui->btn_startStop, SIGNAL( clicked() ), this,
             SLOT( startStop() ) );
    connect( _ui->btn_resetInfo, SIGNAL( clicked() ), this,
             SLOT( resetInfo() ) );
}
Ejemplo n.º 3
0
void Director::cue()
{
	sharedFut = async(launch::async, [&]{
		scriptsIter iter = scripts.begin();
		while (iter != scripts.end()) {
			sIter sIt = iter->second.begin();
			while (sIt != iter->second.end()) {
				play->checkAvailable(maximum);
				for (int i = 0; i < maximum; i++) {
					if (!players[i]->isbusy()) {
						try {
							players[i]->enter(iter->first, sIt->first, sIt->second);
						}
						catch (CodeException& e) {
							// if there is an exception, need to stop all the currently running threads
							emergencyStop();
							throw e;
						}
						break;
					} 
				}
				sIt++;
			}
			iter++;
		}
		
		finished.set_value(true);
		for (int i = 0; i < maximum; i++) {
			players[i]->join();
		}

		return true;
		
	}).share();
}
Ejemplo n.º 4
0
void GRID::checkProcNumber() {
  rnproc[0] = nproc / (rnproc[1] * rnproc[2]);
  if (nproc != (rnproc[0] * rnproc[1] * rnproc[2]))
  {
    printf("BADLY defined number of processors nproc=%i  rnprocx=%i  rnprocy=%i  rnprocz=%i!!!\n", nproc, rnproc[0], rnproc[1], rnproc[2]);
    exit(18);
  }
  if (rnproc[0] < 1 || NGridNodes[0] < 1)
  {
    printf("BADLY defined rnprocx=%i  or  Nx=%i   !!!\n", rnproc[0], NGridNodes[0]);
    exit(18);
  }
  if (rnproc[1] < 1)
  {
    printf("BADLY defined rnprocy=%i  or  Ny=%i   !!!\n", rnproc[1], NGridNodes[1]);
    exit(18);
  }
  if (rnproc[2] < 1)
  {
    printf("BADLY defined rnprocz=%i  or  Nz=%i   !!!\n", rnproc[2], NGridNodes[2]);
    exit(18);
  }

  if (rnproc[0] * 5 > NGridNodes[0]) {
    if (myid == master_proc)
      std::cout << "Too many MPI tasks along x (" << rnproc[0] << ")" << " for " << NGridNodes[0] << " grid points !!!" << std::endl;
    emergencyStop();
  }
  if (dimensions >= 2) {
    if (rnproc[1] * 5 > NGridNodes[1]) {
      if (myid == master_proc)
        std::cout << "Too many MPI tasks along y (" << rnproc[1] << ")" << " for " << NGridNodes[1] << " grid points !!!" << std::endl;
      emergencyStop();
    }
  }
  if (dimensions == 3) {
    if (rnproc[2] * 5 > NGridNodes[2]) {
      if (myid == master_proc)
        std::cout << "Too many MPI tasks along z (" << rnproc[2] << ")" << " for " << NGridNodes[2] << " grid points !!!" << std::endl;
      emergencyStop();
    }
  }

}
Ejemplo n.º 5
0
// printer.testMotors()
void Printer::testMotors() {
	emergencyStop();
	_linearMotionControllerX.singleRevolution();
	_linearMotionControllerY.singleRevolution();
	_linearMotionControllerZ.singleRevolution();

	_linearMotionControllerX.calibrate();
	_linearMotionControllerY.calibrate();

}
Ejemplo n.º 6
0
//--------------------------------------------------------------
void testApp::keyPressed(int key)
{
    cout<<"keypressed : "<<hex<<key<<endl;
    if (key==0x0d)//Return
    {
        emergencyStop();
    }
    if (key==0x72|| key==0x52)//R or r
    {
        reloadXml();
    }
}
Ejemplo n.º 7
0
void GRID::checkStretchedGridInitialization() {
  if (!flagStretched)
    return;
  if (withMovingWindow && isStretchedAlong(0)) {
    if (myid == 0) {
      std::cout << "ERROR: moving window is incompatible with stretching along x!" << std::endl;
      std::cout.flush();

    }
    emergencyStop();
  }
}
Ejemplo n.º 8
0
void elFlowWindow::mLabSignal( char signal, const QString& cmd )
{
    if( signal == SIGNAL_SHUTDOWN )
    {
        emergencyStop();
    }
    else if( signal == SIGNAL_STOP && _port.isOpen() )
    {
        _port.setValue( elFlowPort::setValueType::setTypeFlow, 0, false );

        _ui->lbl_info->setText( STOP_RECEIVED );
        _ui->lbl_info->setStyleSheet( STYLE_ERROR );
        emit changeWindowState( this->windowTitle(), false );
    }
}
Ejemplo n.º 9
0
task usercontrol() {
	initializeTasks();
	setFlywheelRange(0);

	while (true)
	{
		while (vexRT[emergencyStopBtn] == 0)
		{
			setDrivePower(sgn(vexRT[Ch2]) * vexRT[Ch2] * vexRT[Ch2] / 127, sgn(vexRT[Ch3]) * vexRT[Ch3] * vexRT[Ch3] / 127); //drive
			motor[seymore] = abs(targetVelocity - flywheelVelocity) < firingErrorMargin * targetVelocity ? 127*vexRT[fireBtn] - 127*vexRT[seymoreOutBtn] : 0;
			motor[feedMe] = 127*vexRT[feedInBtn] - 127*vexRT[feedOutBtn];
			EndTimeSlice();
		}

		emergencyStop(); //reassign emstop button
	}
}
Ejemplo n.º 10
0
task usercontrol() {
	initializeTasks();
	setFlywheelRange(0);

	while (true)
	{
		while (vexRT[emergencyStopBtn] == 0)
		{
			setDrivePower(sgn(vexRT[Ch2]) * vexRT[Ch2] * vexRT[Ch2] / 127, sgn(vexRT[Ch3]) * vexRT[Ch3] * vexRT[Ch3] / 127); //drive
			motor[feedMe] = 127*vexRT[feedInBtn] - 127*vexRT[feedOutBtn];
			feedMePower = motor[feedMe];
			EndTimeSlice();
		}

		emergencyStop(); //reassign emstop button
	}
}
Ejemplo n.º 11
0
/**
  Check if result is plausible. If it is, an ok is send and the command is stored in queue.
  If not, a resend and ok is send.
*/
void gcode_checkinsert(GCode *act) {
  if(GCODE_HAS_M(act)) {
   if(act->M==110) { // Reset line number
     gcode_lastN = gcode_actN;
     OUT_P_LN("ok");
     return;
   }
   if(act->M==112) { // Emergency kill - freeze printer
     emergencyStop();
   }
  }
  if(GCODE_HAS_N(act)) {
    if((((gcode_lastN+1) & 0xffff)!=(gcode_actN&0xffff))) {
      if(gcode_wait_resend<0) { // after a resend, we have to skip the garbage in buffers, no message for this
        if(DEBUG_ERRORS) {
           OUT_P_L("Error: expected line ",gcode_lastN+1); 
           OUT_P_L_LN(" got ",gcode_actN);
        }
        gcode_resend(); // Line missing, force resend
      } else {
        --gcode_wait_resend;
        gcode_wpos = 0;
        OUT_P_L_LN("skip ",gcode_actN);
        OUT_P_LN("ok");
      }
      return;
    }
    gcode_lastN = gcode_actN;
  }
  gcode_windex = (gcode_windex+1) % GCODE_BUFFER_SIZE;
  gcode_buflen++;
#ifdef ACK_WITH_LINENUMBER  
  OUT_P_L_LN("ok ",gcode_actN);
#else
  OUT_P_LN("ok");
#endif
  gcode_last_binary = gcode_binary;
  gcode_wait_resend = -1; // everything is ok.
#ifndef ECHO_ON_EXECUTE
  if(DEBUG_ECHO) {
      OUT_P("Echo:");
      gcode_print_command(act);
      out.println();
  }
#endif
}
Ejemplo n.º 12
0
void eaps8000UsbWindow::connectUiElements()
{
    connect( _ui->btn_emergencyStop, SIGNAL( clicked() ), this,
             SLOT( emergencyStop() ) );
    connect( _ui->cob_measuredValues, SIGNAL( currentTextChanged( QString ) ),
             this, SLOT( visibilitySelectionChanged() ) );
    connect( _ui->cob_setValue, SIGNAL( currentTextChanged( QString ) ), this,
             SLOT( setValueSelectionChanged() ) );
    connect( _ui->cob_setValueUnit, SIGNAL( currentTextChanged( QString ) ),
             this, SLOT( updateUnitRange() ) );

    connect( _ui->btn_connect, SIGNAL( clicked() ), this,
             SLOT( connectivityButtonPressed() ) );
    connect( _ui->btn_setValue, SIGNAL( clicked() ), this, SLOT( setValue() ) );
    connect( _ui->btn_measuredValuesVisibility, SIGNAL( clicked() ), this,
             SLOT( changeVisibility() ) );
    connect( _ui->btn_resetInfo, SIGNAL( clicked() ), this,
             SLOT( resetInfo() ) );
}
void Smooth_derivative::mainLoop() {
	// determines the number of loops per second
	ros::Rate loop_rate(20);

	// als long as all is O.K : run
	// terminate if the node get a kill signal
	while (m_nodeHandle.ok())
	{
		calculateCommand();
		emergencyStop();

		// send the command to the roomrider for execution
		m_commandPublisher.publish(m_roombaCommand);

		// spinOnce, just make the loop happen once
		ros::spinOnce();
		// and sleep, to be aligned with the 50ms loop rate
		loop_rate.sleep();
	}
}// end of mainLoop
Ejemplo n.º 14
0
bool BaseNode::isEnumVal() const
{
    emergencyStop("isEnumVal", ep);
    return false;
}
Ejemplo n.º 15
0
void testApp::receiveOscMessage()
{
    while(thisOscReceiver.hasWaitingMessages())
    {
        thisOscReceiver.getNextMessage(&thisOscReceivedMessage);
        messageReceived=true;
        if(thisOscReceivedMessage.getAddress()=="/emergencyStop")
        {
            cout<<"received an emergency stop from OSC"<<endl;
            emergencyStop();
        }
        for(int i = 0; i < thisOscReceivedMessage.getNumArgs(); i++)
        {
            cout<<"Received "<<thisOscReceivedMessage.getArgAsFloat(0)<<" on"<<thisOscReceivedMessage.getAddress()<<endl;
            if(thisOscReceivedMessage.getAddress()=="/1/Master")
            {
                float value=thisOscReceivedMessage.getArgAsFloat(0);
                for(vector< ofPtr<dcMotor> >::iterator it = theDcMotors.begin(); it != theDcMotors.end(); ++it)
                {
                    if((*it)->getOnMaster()==true)
                    {
                        (*it)->setSpeed(value);
                    }
                }
            }
            for (int i=0; i<nbDcMotors; i++)

            {
                if(thisOscReceivedMessage.getAddress()=="/1/M"+ofToString(i))
                {
                    float value=thisOscReceivedMessage.getArgAsFloat(0);
                    theDcMotors[i]->setSpeed(value);
                }
                else if(thisOscReceivedMessage.getAddress()=="/1/stop"+ofToString(i))
                {
                    float value=thisOscReceivedMessage.getArgAsFloat(0);
                    if (value==0.0)
                    {
                        theDcMotors[i]->setSpeed(0);
                    }
                }
                else if(thisOscReceivedMessage.getAddress()=="/1/Master"+ofToString(i))
                {
                    float value=thisOscReceivedMessage.getArgAsFloat(0);
                    if (value==0.0)
                    {
                        theDcMotors[i]->setOnMaster(false);
                    }
                    else
                    {
                        theDcMotors[i]->setOnMaster(true);
                    }
                }
                else
                {
                    wrongMess="!!!received "+ofToString(thisOscReceivedMessage.getArgAsFloat(0))+"on"+thisOscReceivedMessage.getAddress();
                }


            }
            for (int i=0; i<nbStepperMotor; i++)
            {
                if(thisOscReceivedMessage.getAddress()=="/4/S"+ofToString(i))
                {
                    float value=thisOscReceivedMessage.getArgAsFloat(0);
                    theSteppers[i]->stepTo(value);
                }
            }
        }
    }

}
Ejemplo n.º 16
0
void eaps8000UsbUICharWindow::mLabSignal( char signal, const QString& cmd )
{
    if( signal == SIGNAL_SHUTDOWN )
    {
        emergencyStop();
    }
    else if( signal == SIGNAL_STOP )
    {
        if( _ui->chb_setZeroAtStopSignal->isChecked() )
        {
            uiCharFinished();

            _ui->lbl_info->setText( STOP_RECEIVED );
            _ui->lbl_info->setStyleSheet( STYLE_ERROR );
        }
    }
    else if( signal == 10 )
    {
        if( _ui->btn_startStop->text() == STOP )
        {
            startStop();
        }
    }
    else if( signal == 11 )
    {
        if( _ui->btn_startStop->text() == START )
        {
            startStop();
        }
    }
    else if( signal == 12 )
    {
        if( _ui->btn_connect->text() == CONNECT_PORT )
        {
            connectPort();
        }
    }
    else if( signal == 13 )
    {
        if( _ui->btn_connect->text() == DISCONNECT_PORT )
        {
            disconnectPort();
        }
    }
    else if( signal == 14 )
    {
        if( cmd == "true" )
        {
            _ui->chb_setZeroAtStopSignal->setChecked( true );
        }
        else if( cmd == "false" )
        {
            _ui->chb_setZeroAtStopSignal->setChecked( false );
        }
    }
    else if( signal == 15 )
    {
        if( cmd == "true" )
        {
            _ui->chb_setZeroWhenFinished->setChecked( true );
        }
        else if( cmd == "false" )
        {
            _ui->chb_setZeroWhenFinished->setChecked( false );
        }
    }
    else if( signal == 16 )
    {
        if( cmd == "true" )
        {
            _ui->chb_emitStopSignal->setChecked( true );
        }
        else if( cmd == "false" )
        {
            _ui->chb_emitStopSignal->setChecked( false );
        }
    }
    else if( signal == 18
             || signal == 19 )
    {
        resetInfo();
    }
    else if( signal == 41 )
    {
        int indexType = _ui->cob_setValue->findText( cmd );
        if( indexType == -1 )
        {
            return;
        }
        _ui->cob_setValue->setCurrentIndex( indexType );
    }
    else if( signal == 42 )
    {
        bool conversionSuccessful = false;
        double stepSize = cmd.toDouble( &conversionSuccessful );
        if( conversionSuccessful )
        {
            _ui->dsb_stepSize->setValue( stepSize );
        }
    }
    else if( signal == 43 )
    {
        if( cmd == "true" )
        {
            _ui->chb_calcValues->setChecked( true );
        }
        else if( cmd == "false" )
        {
            _ui->chb_calcValues->setChecked( false );
        }
    }
    else if( signal == 45 )
    {
        bool conversionSuccessful = false;
        double fromValue = cmd.toDouble( &conversionSuccessful );
        if( conversionSuccessful )
        {
            _ui->dsb_fromValue->setValue( fromValue );
        }
    }
    else if( signal == 46 )
    {
        bool conversionSuccessful = false;
        double toValue = cmd.toDouble( &conversionSuccessful );
        if( conversionSuccessful )
        {
            _ui->dsb_toValue->setValue( toValue );
        }
    }
    else if( signal == 47 )
    {
        int unitType = _ui->cob_unit->findText( cmd );
        if( unitType == -1 )
        {
            return;
        }
        _ui->cob_unit->setCurrentIndex( unitType );
    }
    else if( signal == 51 )
    {
        bool conversionSuccessful = false;
        int repeat = cmd.toInt( &conversionSuccessful );
        if( conversionSuccessful )
        {
            _ui->spb_repeat->setValue( repeat );
        }
    }
    else if( signal == 52 )
    {
        if( cmd == "true" )
        {
            _ui->chb_loop->setChecked( true );
        }
        else if( cmd == "false" )
        {
            _ui->chb_loop->setChecked( false );
        }
    }
}
Ejemplo n.º 17
0
// printer.testEndstopX()
void Printer::testEndstopX() {
	emergencyStop();
	Serial.println("Send \"printer.emergencyStop()\\n\" to stop polling.");
	_actionTestingEndstopX = true;
}
Ejemplo n.º 18
0
/*!
**
**
** @param d
** @param newCommunicationState
**/
void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)
{
#ifdef CO_ENABLE_LSS
	StartOrStop(csLSS,	startLSS(d),	stopLSS(d))
#endif
	StartOrStop(csSDO,	None,		resetSDO(d))
	StartOrStop(csSYNC,	startSYNC(d),		stopSYNC(d))
	StartOrStop(csHeartbeat,	heartbeatInit(d),	heartbeatStop(d))
	StartOrStop(csEmergency,	emergencyInit(d),	emergencyStop(d))
	StartOrStop(csPDO,	PDOInit(d),	PDOStop(d))
	StartOrStop(csBoot_Up,	None,	slaveSendBootUp(d))
}

/*!
**
**
** @param d
** @param newState
**
** @return
**/
UNS8 setState(CO_Data* d, e_nodeState newState)
{
	if (newState != d->nodeState) {
Ejemplo n.º 19
0
void eaps8000UsbWindow::mLabSignal( char signal, const QString& cmd )
{
    if( signal == SIGNAL_SHUTDOWN )
    {
        emergencyStop();
    }
    else if( signal == SIGNAL_STOP )
    {
        if( _ui->chb_setZeroAtStopSignal->isChecked()
                && _port.isOpen() )
        {
            _port.setValue( eaps8000UsbPort::setValueType::setTypeVoltage, 0.0,
                            false );
            _port.setValue( eaps8000UsbPort::setValueType::setTypeCurrent, 0.0,
                            false );

            _ui->lbl_info->setText( STOP_RECEIVED );
            _ui->lbl_info->setStyleSheet( STYLE_ERROR );
            emit changeWindowState( this->windowTitle(), false );
        }
    }
    else if( signal == 12 )
    {
        if( _ui->btn_connect->text() == CONNECT_PORT )
        {
            connectPort();
        }
    }
    else if( signal == 13 )
    {
        if( _ui->btn_connect->text() == DISCONNECT_PORT )
        {
            disconnectPort();
        }
    }
    else if( signal == 18
             || signal == 19 )
    {
        resetInfo();
    }
    else if( signal == 30 )
    {
        if( _port.isRunning() )
        {
            _ui->dsp_setValue->setValue( 0.0 );
            _ui->chb_adjustSetValue->setChecked( false );
            _port.setValue( eaps8000UsbPort::setValueType::setTypeVoltage,
                            0.0, false );
            _port.setValue( eaps8000UsbPort::setValueType::setTypeCurrent,
                            0.0, false );
        }
    }
    else if( signal >= 31
             && signal <= 39 )
    {
        if( !_port.isRunning() )
        {
            return;
        }
        if( cmd.at( 0 ) != 'a'
                && cmd.at( 0 ) != 'n' )
        {
            return;
        }
        bool adjustValue = false;
        if( cmd.at( 0 ) == 'a' )
        {
            adjustValue = true;
        }
        bool convOk = false;
        double value = cmd.mid( 1 ).toDouble( &convOk );
        if( !convOk )
        {
            return;
        }

        QString type;
        QString unit;
        if( signal == 31 )
        {
            type = VOLTAGE;
            unit = UNIT_VOLT;
        }
        else if( signal == 32 )
        {
            type = CURRENT;
            unit = UNIT_AMPERE;
        }
        else if( signal == 33 )
        {
            type = POWER;
            unit = UNIT_WATT;
        }
        else if( signal == 34 )
        {
            type = POWER_BY_VOLTAGE;
            unit = UNIT_WATT;
        }
        else if( signal == 35 )
        {
            type == POWER_BY_CURRENT;
            unit = UNIT_WATT;
        }
        else if( signal == 36 )
        {
            type == RESISTANCE_BY_VOLTAGE;
            unit = UNIT_OHM;
        }
        else if( signal == 37 )
        {
            type == RESISTANCE_BY_CURRENT;
            unit = UNIT_OHM;
        }
        else if( signal == 38 )
        {
            type = _ui->cob_setValue->currentText();
            unit = _ui->cob_setValueUnit->currentText();
            value += _ui->dsp_setValue->value();
        }
        else if( signal == 39 )
        {
            type = _ui->cob_setValue->currentText();
            unit = _ui->cob_setValueUnit->currentText();
            value *= _ui->dsp_setValue->value();
        }

        int indexType = _ui->cob_setValue->findText( type );
        int indexUnit = _ui->cob_setValue->findText( unit );
        if( indexType == -1
                || indexUnit == -1 )
        {
            return;
        }
        _ui->dsp_setValue->setValue( value );
        _ui->cob_setValue->setCurrentIndex( indexType );
        _ui->cob_setValueUnit->setCurrentIndex( indexUnit );
        _ui->chb_adjustSetValue->setChecked( adjustValue );
        setValue();
    }
}
Ejemplo n.º 20
0
bool BaseNode::isConstant() const
{
    emergencyStop("isConstant", ep);
    return false;
}
Ejemplo n.º 21
0
// printer.emergencyStop()
void Printer::emergencyStop()
{
	emergencyStop(0);
}
Ejemplo n.º 22
0
void EmssController::run() {
	// Enter processing loop
	stopRequested = false;
	while (stopRequested == false) {

		// Get movement...
		double distanceDelta = coil_getDistance();
		double angleDelta = coil_getAngle();

		// Emit signals for movement tracker
		emit signalMovedDistance(distanceDelta);
		emit signalChangedAngle(angleDelta);

		// Get other sensor data
		int sharpIRSensor = coil_getAnalogSensorDistance();
		int bumpsWheelDrop = coil_getBumpsAndWheelDrops();

		// Collision?
		if (((COIL::BUMPWHEELDROP_BUMP_LEFT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_LEFT) || ((COIL::BUMPWHEELDROP_BUMP_RIGHT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_RIGHT)) {
			emit signalCollision();
			if(create->boolSetting("EMSSCONTROLLER_EMERGENCY_STOP_ENABLED") == true) emergencyStop();
		}

		// Object detected?
		if (sharpIRSensor < create->intSetting("EMSSCONTROLLER_SHARP_IR_SENSOR_CUTOFF_VALUE")) {
			emit signalObjectDetected(sharpIRSensor, 0); // Angle is 0 because it is strait ahead always!
			if(create->boolSetting("EMSSCONTROLLER_EMERGENCY_STOP_ENABLED") == true && sharpIRSensor < create->intSetting("EMSSCONTROLLER_SHARP_IR_SENSOR_EMERGENCYSTOP_BUFFER_MM")) emergencyStop();
		}

		// Processs movement
		distanceMoved += distanceDelta;
		angleTurned += angleDelta;

		// Determine wheel speeds
		if (mode == EmssController::Idle || mode == EmssController::EmergencyStop) {

			// Idle mode!

			Lwheel = 0;
			Rwheel = 0;

		} else if (mode == EmssController::Joystick) {

			// Joystick mode!

			if (this->yokeY == 0) {
				// Left or right
				Lwheel = (short) (this->speed * this->yokeX);
				Rwheel = -(short) (this->speed * this->yokeX);

			} else {
				// Move forwards backwards
				Lwheel = (short) (this->speed * this->yokeY);
				Rwheel = (short) (this->speed * this->yokeY);
			}

		} else if (mode == EmssController::Move) {

			// Move mode!

			Lwheel = speed;
			Rwheel = speed;

		} else if (mode == EmssController::Turn) {

			// Turn mode!

			if (angleToTurn > 0) {
				Lwheel = -speed;
				Rwheel = +speed;
			} else {
				Lwheel = +speed;
				Rwheel = -speed;
			}
			//Debug::print("[EmssController] debug");


		} else if (mode == EmssController::WheelDrive) {

			// No change, just drive at current wheel values...
		}

		// Send wheel speeds to COIL
		if(mode != EmssController::EmergencyStop) coil_directDrive(Lwheel, Rwheel);


		//Debug::print("[EmssController] debug");
		// Sleep our interval...
		this->msleep(interval);

	}
}
Ejemplo n.º 23
0
void Printer::loop(unsigned long now)
{
	if (_emergencyStop) {
		emergencyStop();
	}

	_linearMotionControllerX.loop(now);
	_linearMotionControllerY.loop(now);
	_linearMotionControllerZ.loop(now);

	if (_actionTestingEndstopX) {
		if (_actionCycles == 2000) {
			EndstopSwitch * _endstop = (Ramps::instance()).getEndstopX();
			Serial.println(_endstop->triggered()?"true":"false");
			_actionCycles = 1;
		}
	}

	if (_actionTestingHeatbed) {
		if (_actionCycles == 0) {
			Serial.println("Testing heatbed");
			(Ramps::instance()).setHeatbedTemp(60);
		} else if (_actionCycles == 2000) {
			int temp = (Ramps::instance()).pollHeatbed();
			Serial.write("Heatbed Temp: (deg C) ");
			Serial.println(temp);
			
			if (temp >= 60) {
				stopTestingHeatbed();
				Serial.println("ready");
			}
			_actionCycles = 1;

		}
	}

	if (_actionTestingExtruder) {
		
		ExtruderController* extruder;

		if (_actionCycles == 0 || _actionCycles == 2000 || _actionCycles == 40000) {
			extruder = (Ramps::instance()).getExtruderA();
		}

		if (_actionCycles == 0) {
			Serial.println("Testing extruder");
			extruder->setTemp(180);
		} else if (_actionCycles == 2000) {
			int temp = extruder->getTemp();
			Serial.write("Extruder Temp: (deg C) ");
			Serial.println(temp);
			
			if (temp >= 180) {
				extruder->setRate(40);
			} else {
				_actionCycles = 1;
			}
		} else if (_actionCycles == 40000) {
			_actionTestingExtruder = false;
			extruder->setRate(0);
			extruder->setTemp(0);
			Serial.println("ready");
		}
	}

	_actionCycles++;
}
Ejemplo n.º 24
0
RTC::ReturnCode_t ArmControlCartesian::onExecute(RTC::UniqueId ec_id)
{
  //
  // asigned part index
  //
  int aid = m_assignedPartIndex;


  //
  // get data from in port & update robot model
  //
  if(m_jointDatIn.isNew ()) {
    m_jointDatIn.read();
    for(unsigned int i = 0; i < m_partMembers.size (); i++) {
      for(unsigned int j = 0; j < m_partMembers[i].size (); j++) {
	hrp::Link * l = m_robot->joint((int) m_partMembers[i][j]);
	if(l != NULL)
	  l->q = m_jointDat.qState[i][j];
      }
    }
    m_robot->calcForwardKinematics ();
  }
  else
    return RTC::RTC_OK;


  //
  // check reference data
  //
  if( isEmpty() )
    return RTC::RTC_OK;


  //
  // set data for not asigined part
  //
  for(int i=0; i<m_jointDat.id.length(); i++) {
    if(i != aid) {
      m_jointDat.id[i] = CMD_IDLE;
      m_jointDat.cmd[i].sec = 0;
      m_jointDat.cmd[i].nsec = 0;
    }
  }


  //
  // clear motion
  //
  if (  m_jointDat.id[aid] == CMD_GO_ACTUAL
	|| m_jointDat.id[aid] == CMD_PROTECTIVE_STOP
	|| m_cmdTimes[aid].sec  <  m_jointDat.cmd[aid].sec
	|| (m_cmdTimes[aid].sec  == m_jointDat.cmd[aid].sec &&
	    m_cmdTimes[aid].nsec <  m_jointDat.cmd[aid].nsec))
    {
      emergencyStop();
      m_jointDat.id[aid] = CMD_PROTECTIVE_STOP;
      m_jointDat.cmd[aid].sec  = m_cmdTimes[aid].sec;
      m_jointDat.cmd[aid].nsec = m_cmdTimes[aid].nsec;
      m_jointDatOut.write ();
      return RTC::RTC_OK;
    }

  //
  // write to out port
  //
  else
    {
      // calc inverse kinematics
      hrp::Vector3  ref_p;   m_pos_interpolator->get(&ref_p[0]);
      hrp::Matrix33 ref_R0;  m_rot_interpolator->get(ref_R0);
      hrp::Matrix33 ref_R1 = m_jointPath->endLink()->calcRfromAttitude(ref_R0);
      if( !m_jointPath->calcInverseKinematics(ref_p, ref_R1) ) {
	emergencyStop();
	return RTC::RTC_OK;
      }

      // update port dat
      for(int i = 0; i < m_jointPath->numJoints(); i++)
	m_jointDat.qCommand[aid][i] = m_jointPath->joint(i)->q;

      m_jointDat.id[aid]       = m_cmdId;
      m_jointDat.cmd[aid].sec  = m_cmdTimes[aid].sec;
      m_jointDat.cmd[aid].nsec = m_cmdTimes[aid].nsec;

      setCurrentTime (m_jointDat.tm);
      m_jointDatOut.write ();
      return RTC::RTC_OK;
    }


  return RTC::RTC_OK;
}
Ejemplo n.º 25
0
void EmssController::process() {

	// Determine wheel speeds
	if (mode == EmssController::Idle || mode == EmssController::EmergencyStop) {

		// Idle mode!

		Lwheel = 0;
		Rwheel = 0;


	} else if (mode == EmssController::WheelDrive) {

		// No change, just drive at current wheel values...
	}

	// Get all the sensor data...
	if(sensorData) {
		if(create->coil->getAllSensors(sensorData)) {
			emit signalSensorDataUpdated();
		} else {
			// Return out of the process until Core can process the
			// signal from COIL. This will help COIL disconnect
			// faster...
			return;
		}
	}

	// Get movement...
	double distanceDelta = create->coil->extractSensorFromData(sensorData,COIL::SENSOR_DISTANCE);
	double angleDelta = create->coil->extractSensorFromData(sensorData,COIL::SENSOR_ANGLE);

	// Emit signals for movement tracker
	emit signalMovedDistance(distanceDelta);
	emit signalChangedAngle(angleDelta);

	// Get other sensor data
	int sharpIRSensor = create->arduino->extractSensorFromData(create->arduinoController->sensorData, ArduinoCOIL::SENSOR_IR_2);
	sharpIRSensor *= 25.4;
//	int sharpIRSensor = create->coil->getIRSensorDistanceFromAnalogSignal(create->coil->extractSensorFromData(sensorData,COIL::SENSOR_ANALOG_SIGNAL));
	int wallIRSensor = create->coil->getWallSensorDistanceFromSignal(create->coil->extractSensorFromData(sensorData,COIL::SENSOR_WALL_SIGNAL));
	int bumpsWheelDrop = create->coil->extractSensorFromData(sensorData,COIL::SENSOR_BUMPS_AND_WHEEL_DROPS);
	bool cliffLeft = create->coil->extractSensorFromData(sensorData,COIL::SENSOR_CLIFF_LEFT);
	bool cliffFrontLeft = create->coil->extractSensorFromData(sensorData,COIL::SENSOR_CLIFF_FRONT_LEFT);
	bool cliffFrontRight = create->coil->extractSensorFromData(sensorData,COIL::SENSOR_CLIFF_FRONT_RIGHT);
	bool cliffRight = create->coil->extractSensorFromData(sensorData,COIL::SENSOR_CLIFF_RIGHT);
	bool isForwardsDirection = (Lwheel > 0 && Rwheel > 0);

	//ebug::print("[EmssController] sensor: %1", sharpIRSensor);

	// Drop detected?
	if (cliffLeft || cliffFrontLeft || cliffFrontRight || cliffRight) {
		int angle;
		if (cliffLeft){
			angle = create->intSetting("Robot_SideCliffSensorPositionAngle");
			emit signalObjectDetected(bumperCollisionOffset, angle, cliffCollisionOpacity, cliffCollisionSize );
		} else if (cliffFrontLeft){
			angle = create->intSetting("Robot_FrontCliffSensorPositionAngle");
			emit signalObjectDetected(bumperCollisionOffset, angle, cliffCollisionOpacity, cliffCollisionSize );
		} else if (cliffFrontRight){
			angle = -create->intSetting("Robot_FrontCliffSensorPositionAngle");
			emit signalObjectDetected(bumperCollisionOffset, angle, cliffCollisionOpacity, cliffCollisionSize );
		} else if (cliffRight){
			angle = -create->intSetting("Robot_SideCliffSensorPositionAngle");
			emit signalObjectDetected(bumperCollisionOffset, angle, cliffCollisionOpacity, cliffCollisionSize );
		}
		if(isForwardsDirection && create->boolSetting("Controller_EmssController_EmergencyStopEnabled")) emergencyStop();
	}

	// Bumper Collision?
	if (((COIL::BUMPWHEELDROP_BUMP_LEFT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_LEFT) || ((COIL::BUMPWHEELDROP_BUMP_RIGHT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_RIGHT)) {
		int angle = 0;
		// both bumper?
		if (((COIL::BUMPWHEELDROP_BUMP_LEFT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_LEFT) && ((COIL::BUMPWHEELDROP_BUMP_RIGHT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_RIGHT)){
			angle = 0;
		}
		// only left bumper?
		else if ((COIL::BUMPWHEELDROP_BUMP_LEFT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_LEFT){
			angle = 45;
		}
		// only right bumper?
		else if ((COIL::BUMPWHEELDROP_BUMP_LEFT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_LEFT){
			angle = -45;
		}
		emit signalObjectDetected(bumperCollisionOffset, angle, bumperCollisionOpacity, bumperCollisionSize );
		if(isForwardsDirection && emergencyStopEnabled) emergencyStop();
	}

	// Wall detected?
	if(wallIRSensor < robotWallSensorRange) {
		emit signalObjectDetected(robotDiameter/2 + wallIRSensor, -90, wallCollisionOpacity, wallCollisionSize ); // Angle is -90 because the sensor points straight out to the right...
	}

	// Object detected?



	if (sharpIRSensor < create->intSetting("Robot_SharpIRSensorCutoffValue")) {
		emit signalObjectDetected(sharpIRSensor, 0, irCollisionOpacity, irCollisionSize ); // Angle is 0 because it is straight ahead always!
	}

	// Send wheel speeds to COIL
	if(mode != EmssController::EmergencyStop) {
		// Set new wheel speed
		create->coil->directDrive(Lwheel, Rwheel);
		emit signalChangedWheelSpeed((int)Lwheel, (int)Rwheel);

		// Register on heat map as safe area
		create->heatMap->registerHeat(ColorMap::OpenAreaChannel, create->tracker->getX(), create->tracker->getY(), openAreaOpacity, openAreaSize);
	}

	// Time for debug info?
	if(debugInfoEnabled && lastDebugInfo.elapsed() > debugInfoInterval) {
		lastDebugInfo.restart();
		Debug::print("[EmssController] vl=%1\tvr=%2", Lwheel, Rwheel);
	}

}