// ----------------------------------------------------------------------------
// 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;
}
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() ) );
}
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();
}
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();
}
}
}
// printer.testMotors()
void Printer::testMotors() {
emergencyStop();
_linearMotionControllerX.singleRevolution();
_linearMotionControllerY.singleRevolution();
_linearMotionControllerZ.singleRevolution();
_linearMotionControllerX.calibrate();
_linearMotionControllerY.calibrate();
}
//--------------------------------------------------------------
void testApp::keyPressed(int key)
{
cout<<"keypressed : "<<hex<<key<<endl;
if (key==0x0d)//Return
{
emergencyStop();
}
if (key==0x72|| key==0x52)//R or r
{
reloadXml();
}
}
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();
}
}
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 );
}
}
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
}
}
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
}
}
/**
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
}
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
bool BaseNode::isEnumVal() const
{
emergencyStop("isEnumVal", ep);
return false;
}
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);
}
}
}
}
}
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 );
}
}
}
// printer.testEndstopX()
void Printer::testEndstopX() {
emergencyStop();
Serial.println("Send \"printer.emergencyStop()\\n\" to stop polling.");
_actionTestingEndstopX = true;
}
/*!
**
**
** @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) {
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();
}
}
bool BaseNode::isConstant() const
{
emergencyStop("isConstant", ep);
return false;
}
// printer.emergencyStop()
void Printer::emergencyStop()
{
emergencyStop(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);
}
}
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++;
}
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;
}
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);
}
}
