task main () {
bFloatDuringInactiveMotorPWM = true;
int numDegrees = 0;
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "Compass");
nxtDisplayCenteredTextLine(3, "Test 2");
nMotorEncoder[M_RIGHT] = 0;
nMotorEncoder[M_LEFT] = 0;
// This will make the robot spin about 1.5 times, depends on many factors, YYMV, etc
numDegrees = ((numRotations() * 3) / 2) * 450;
StartTask(timeMe);
startCalibration();
nxtDisplayCenteredTextLine(5, "Calibrating...");
StartTask(showPulse);
motor[M_RIGHT] = MOTORSPEED;
motor[M_LEFT] = -MOTORSPEED;
while(nMotorEncoder[M_RIGHT] < numDegrees) wait1Msec(5);
motor[M_LEFT] = 0;
motor[M_RIGHT]= 0;
stopCalibration();
nxtDisplayCenteredTextLine(5, "Calibration done");
wait1Msec(5000);
}
void ofxMultiplexerManager::cancelCalibration()
{
if (calibration->calibrate.bCalibrating)
{
for (int i=0;i<cameraBasesCalibration.size();i++)
{
cameraBasesCalibration[i]->calibrationPoints.clear();
int calibrationPointWidth = calibrationGridWidth + 1;
int calibrationPointHeight = calibrationGridHeight + 1;
float cameraCellWidth = 1.0f / calibrationGridWidth;
float cameraCellHeight = 1.0f / calibrationGridHeight;
for (int j = 0;j<calibrationPointWidth*calibrationPointHeight;j++)
{
vector2df newPoint;
newPoint.X = cameraCellWidth * (j%(calibrationGridWidth+1));
newPoint.Y = cameraCellHeight * (j/(calibrationGridWidth+1));
cameraBasesCalibration[i]->calibrationPoints.push_back(newPoint);
}
}
stopCalibration();
}
else
{
cameraMultiplexer->initializeMultiplexer();
cameraMultiplexer->startStreamingFromAllCameras();
cameraMultiplexer->updateStitchedFrame();
}
filters->bLearnBakground = true;
}
void ofxMultiplexerManager::updateCalibrationStatus()
{
if (needToUpdateBackground)
{
if (calibration->calibrate.selectedCameraIndex > 0)
{
cameraMultiplexer->pauseStreamingFromAllCameras();
cameraMultiplexer->startStreamingFromCamera(calibration->calibrate.selectedCameraIndex);
cameraMultiplexer->updateStitchedFrame();
}
filters->bLearnBakground = true;
needToUpdateBackground = false;
}
if (calibration->calibrate.bNextCamera)
{
cameraMultiplexer->setCalibrationPointsToCamera(calibration->calibrate.selectedCameraIndex,calibration->calibrate.cameraPoints);
calibration->calibrate.nextCameraCalibration();
if (calibration->calibrate.selectedCameraIndex == 0)
{
stopCalibration();
}
needToUpdateBackground = true;
}
}
void SensorManager::devicePSMStateChanged(bool psmState)
{
if (psmState)
{
emit stopCalibration();
}
}
//------------------------------------------------------------------------------------------------
void CalibrationWidget::on_pushButton_kalibrieren_clicked()
{
if (calibrationRunning) // calibration is already running --> stop it
stopCalibration();
else // calibration is not running --> start it
startCalibration();
}
void CompassMotorCalibrationDialog::activeUASSet(UASInterface *uas)
{
if (m_uasInterface){
disconnect(ui->startButton, SIGNAL(clicked()), this, SLOT(startCalibration()));
disconnect(ui->stopButton, SIGNAL(clicked()), this, SLOT(stopCalibration()));
disconnect(m_uasInterface, SIGNAL(compassMotCalibration(mavlink_compassmot_status_t)),
this, SLOT(compassMotCalibration(mavlink_compassmot_status_t)));
disconnect(m_uasInterface, SIGNAL(textMessageReceived(int,int,int,QString)),
this, SLOT(textMessageReceived(int,int,int,QString)));
}
m_uasInterface = uas;
if(m_uasInterface){
connect(ui->startButton, SIGNAL(clicked()), this, SLOT(startCalibration()));
connect(ui->stopButton, SIGNAL(clicked()), this, SLOT(stopCalibration()));
connect(m_uasInterface, SIGNAL(compassMotCalibration(mavlink_compassmot_status_t*)),
this, SLOT(compassMotCalibration(mavlink_compassmot_status_t*)));
}
}
void ofxNetworkSyncClientState::threadedFunction(){
while(isThreadRunning()){
if(step == CALIBRATING && calibrator.isFinishMeasuremant()){
stopCalibration();
}
if(! isConnected()){
ofNotifyEvent(clientDisconnected, clientId, this);
break;
}
string recv = tcpServer->receive(clientId);
if(recv.length() > 0){
ofNotifyEvent(messageReceived, recv, this);
server->onClientMessageReceived(clientId, recv);
}
ofSleepMillis(10);
}
}
int main(int argc, char *argv[])
{
QCoreApplication app(argc, argv);
SensorManager& sm = SensorManager::instance();
Parser parser(app.arguments());
SensordLogger::init(parser.logTarget(), parser.logFilePath(), "sensord");
if (parser.printHelp())
{
printUsage();
app.exit(EXIT_SUCCESS);
return 0;
}
if (parser.changeLogLevel())
{
SensordLogger::setOutputLevel(parser.getLogLevel());
}
const char* CONFIG_FILE_PATH = "/etc/sensorfw/sensord.conf";
const char* CONFIG_DIR_PATH = "/etc/sensorfw/sensord.conf.d/";
QString defConfigFile = CONFIG_FILE_PATH;
if(parser.configFileInput())
{
defConfigFile = parser.configFilePath();
}
QString defConfigDir = CONFIG_DIR_PATH;
if(parser.configDirInput())
{
defConfigDir = parser.configDirPath();
}
if (!Config::loadConfig(defConfigFile, defConfigDir))
{
sensordLogC() << "Config file error! Load using default paths.";
if (!Config::loadConfig(CONFIG_FILE_PATH, CONFIG_DIR_PATH))
{
sensordLogC() << "Which also failed. Bailing out";
return 1;
}
}
signal(SIGUSR1, signalUSR1);
signal(SIGUSR2, signalUSR2);
signal(SIGINT, signalINT);
#ifdef PROVIDE_CONTEXT_INFO
if (parser.contextInfo())
{
sensordLogD() << "Loading ContextSensor " << sm.loadPlugin("contextsensor");
sensordLogD() << "Loading ALSSensor " << sm.loadPlugin("alssensor");
}
#endif
if (parser.createDaemon())
{
int pid = fork();
if(pid < 0)
{
sensordLogC() << "Failed to create a daemon: " << strerror(errno);
exit(EXIT_FAILURE);
} else if (pid > 0) {
sensordLogW() << "Created a daemon";
exit(EXIT_SUCCESS);
}
}
if (parser.magnetometerCalibration())
{
CalibrationHandler* calibrationHandler_ = new CalibrationHandler(NULL);
calibrationHandler_->initiateSession();
QObject::connect(&sm, SIGNAL(resumeCalibration()), calibrationHandler_, SLOT(resumeCalibration()));
QObject::connect(&sm, SIGNAL(stopCalibration()), calibrationHandler_, SLOT(stopCalibration()));
}
if (!sm.registerService())
{
sensordLogW() << "Failed to register service on D-Bus. Aborting.";
exit(EXIT_FAILURE);
}
int ret = app.exec();
sensordLogD() << "Exiting...";
Config::close();
SensordLogger::close();
return ret;
}
//------------------------------------------------------------------------------------------------
CalibrationWidget::~CalibrationWidget()
{
if (calibrationRunning)
stopCalibration();
}
