void Satin::startProcessing() { time_t the_time; ofstream fd; char outputFile[N][9], dischargePressure[N][3], carbonDioxide[N][3]; double startTime, timeDifference; float smallSignalGain[N]; int pNum, lNum, inputPowerData[N]; startTime = (double) clock() / CLOCKS_PER_SEC; pNum = getInputPowers(inputPowerData); lNum = getLaserData(smallSignalGain, outputFile, dischargePressure, carbonDioxide); for (int i = 0; i < lNum; i++) { fd.open(outputFile[i], ios::out); if (fd.fail()) { cout << "Error opening " << outputFile[i] << "\n"; exit(1); } time(&the_time); fd << "Start date: " << ctime(&the_time) << "\nGaussian Beam\n\n" << "Pressure in Main Discharge = " << dischargePressure[i] << "kPa\n" << "Small-signal Gain = " << smallSignalGain[i] << "\nCO2 via " << carbonDioxide[i] << "\n\nPin\t\tPout\t\tSat. Int.\tln(Pout/Pin)\tPout-Pin\n" << "(watts)\t\t(watts)\t\t(watts/cm2)\t\t\t(watts)\n"; fd.flush(); fd.close(); for (int j = 0; j < pNum; j++) { gaussianCalculation(inputPowerData[j], smallSignalGain[i], outputFile[i]); } fd.open(outputFile[i], ios::in | ios::app); time(&the_time); fd << "\nEnd date: " << ctime(&the_time); fd.flush(); fd.close(); } timeDifference = ((double) clock() / CLOCKS_PER_SEC) - startTime; cout << "The CPU time was " << setiosflags(ios::fixed) << setprecision(2) << timeDifference << " seconds.\n"; }
void Api::RunNavigationAlgorithm() { double v, w, l, pan, tilt; jderobot::LaserDataPtr laser; CvPoint2D32f dest; int i, j; jderobot::EncodersDataPtr encoders; int cont = 0; /*A PARTIR DE AQUÍ SE PUEDE AÑADIR EL CÓDIGO DE NAVEGACIÓN IMPLEMENTADO POR EL ESTUDIANTE*/ /*ALGUNOS EJEMPLOS*/ /*Manipulando imágenes de las cámaras*/ /*En el siguiente ejemplo se filtra el color rojo de la cámara izquierda para repintar esos píxeles a negro. Para visualizar el resultado debemos desplegar la ventana "WINDOW DEBUGGING" y pulsar PLAY para hacer correr nuestro código*/ imageCameras2openCV(); //Esta función es necesario llamarla ANTES de trabajar con las imágenes de las cámaras. cv::line(imageCameraLeft, cv::Point2d(0,0), cv::Point2d(imageCameraLeft.cols, imageCameraLeft.rows), cv::Scalar::all(255), 3); /* A continuacion se muestran las coordenadas de los puntos obtenidos tras hacer click en alguna de las camaras */ //std::cout << x_click_cameraleft << std::endl; // Coordenada x del punto donde se ha hecho click en la camara izquierda //std::cout << y_click_cameraleft << std::endl; // Coordenada y del punto donde se ha hecho click en la camara izquierda //std::cout << x_click_cameraright << std::endl; // Coordenada x del punto donde se ha hecho click en la camara derecha //std::cout << y_click_cameraright << std::endl; // Coordenada y del punto donde se ha hecho click en la camara derecha /* EJEMPLO SENCILLO DE UN BUMP AND GO */ /* TOMA DE SENSORES */ //Aqui tomamos el valor de los sensores para alojarlo en nuestras variables locales laser = getLaserData(); // Get the laser info //printf("laser[45]: %d\n", laser->distanceData[45]); v = this->getMotorV(); w = this->getMotorW(); l = this->getMotorL(); // printf("v: %f , w: %f , l: %f\n", v, w, l); //dest = this->getDestino(); //printf("destPoint = [%f, %f]\n", dest.x, dest.y); //encoders = this->getEncodersData(); //printf("myPosition = [%f, %f]\n", encoders->robotx, encoders->roboty); /* FIN TOMA DE SENSORES */ /* switch(accion){ case 0: // Robot hacia adelante if(( laser->distanceData[45] < 1000.0) or ( laser->distanceData[90] < 1000.0) or ( laser->distanceData[135] < 1000.0)){ v=0.; //if ((x_ant == myPoint.x) and (y_ant == myPoint.y) and (z_ant == myPoint.z)){ accion=1; printf("### Activado hacia Atras\n"); //} } else v=50; break; case 1: // Robot hacia atras if ((laser->distanceData[45] < 1100) or (laser->distanceData[90] < 1100) or (laser->distanceData[135] < 1100)){ v=-50.; printf("### Llendo hacia atras\n"); } else{ v=0.; accion=2; } break; case 2: // Robot girando. if((laser->distanceData[45] < 1300) or (laser->distanceData[90] < 1300) or (laser->distanceData[135] < 1300)){ if(sentido%2==0){ w=50.; } else{ w=50.*(-1); } printf("### Girando: %d \n", sentido); } else{ w=0.; accion=0; sentido = (1 + rand() % 40); } break; } */ /*Comandar robot*/ //Aqui enviamos los datos al robot //w = 0.; this->setMotorW(3); this->setMotorV(0); this->setMotorL(0); //pan=0; //tilt=0; //this->setPTEncoders(pan,tilt,1); //Con el segundo parametro elegimos la camara a comandar (1 o 2) }
/** * @brief RobotThread::RobotThread */ RobotThread::RobotThread() { // necessary to send this type over qued connections qRegisterMetaType< QVector<double> >("QVector<double>"); qRegisterMetaType< QList<Obstacle> >("QList<Obstacle>"); qRegisterMetaType< QList< QPair<double,double> > >("QList< QPair<double,double> >"); // fuer path planning -> path realizer qRegisterMetaType< PathPlotData >("PathPlotData"); qRegisterMetaType< FilterParams >("FilterParams"); qRegisterMetaType< CameraParams >("CameraParams"); qRegisterMetaType< PIDParams >("PIDParams"); qRegisterMetaType< PIDPlotData >("PIDPlotData"); qRegisterMetaType< cv::Mat >("cv::Mat"); qRegisterMetaType< QString >("QString"); //Log qRegisterMetaType< LaserPlotData > ("LaserPlotData"); qRegisterMetaType< Position > ("Position"); qRegisterMetaType< TeamColor >("TeamColor"); qRegisterMetaType< CamColor >("CamColor"); qDebug() << "INIT 'RobotThread'"; //initialize Objects for Actors and Sensory actorLowLevel = new ActorLowLevel(); actorHighLevel = new ActorHighLevel(); sensorHighLevel = new SensorHighLevel(); sensorLowLevel = new SensorLowLevel(); pathPlanner = new PathPlanning(); gameEngine = new GameEngine(); cam = new Cam(); // moving objects to different threads sensorLowLevel->moveToThread(&threadRobotLowLevel); sensorHighLevel->moveToThread(&threadSensorHighLevel); actorLowLevel->moveToThread(&threadRobotLowLevel); actorHighLevel->moveToThread(&threadActorHighLevel); pathPlanner->moveToThread(&threadPathPlanner); gameEngine->moveToThread(&threadGameEngine); cam->moveToThread(&threadCam); // Name the threads (helpful for debugging) threadRobotLowLevel.setObjectName("Robot Low Level Thread"); threadSensorHighLevel.setObjectName("High Level Sensor Thread"); threadPathRealizer.setObjectName("Path Realizer Thread"); threadPathPlanner.setObjectName("Path Planner Thread"); threadGameEngine.setObjectName("Game Engine Thread"); threadCam.setObjectName("Cam Thread"); // *********************************************************************** // DELETE LATER connect(&threadRobotLowLevel, SIGNAL(finished()), sensorLowLevel, SLOT(deleteLater())); connect(&threadSensorHighLevel, SIGNAL(finished()), sensorHighLevel, SLOT(deleteLater())); connect(&threadRobotLowLevel, SIGNAL(finished()), actorLowLevel, SLOT(deleteLater())); connect(&threadActorHighLevel, SIGNAL(finished()), actorHighLevel, SLOT(deleteLater())); connect(&threadPathPlanner, SIGNAL(finished()), pathPlanner, SLOT(deleteLater())); connect(&threadCam, SIGNAL(finished()), cam, SLOT(deleteLater())); connect(&threadGameEngine, SIGNAL(finished()), gameEngine, SLOT(deleteLater())); // *********************************************************************** // SIGNALS FROM GUI // start Orientation connect(mainWindow,SIGNAL(signalStartOrientation(bool)), sensorHighLevel, SLOT(slotStartDetection(bool))); // remote controle over GUI connect(mainWindow,SIGNAL(robotRemoteControllUpdate(double,double)), actorLowLevel,SLOT(setRobotRemoteControllParams(double,double))); // remote Odometry update connect(mainWindow,SIGNAL(updateRemoteOdometry(Position)), actorLowLevel, SLOT(setOdometry(Position))); // Cam Display // Camera Parameters in GUI have changed, send to Cam class connect(mainWindow, SIGNAL(signalChangeCamParams(CameraParams)), cam, SLOT(slotSetCameraParams(CameraParams))); // Set Median Filter connect(mainWindow, SIGNAL(signalChangeFilterParams(FilterParams)), sensorHighLevel, SLOT(slotSetFilterParams(FilterParams))); // PID Tab connect(mainWindow, SIGNAL(signalChangePIDParams(PIDParams)), actorHighLevel, SLOT(slotChangePIDParams(PIDParams))); // *********************************************************************** // SIGNALS TO GUI // Sensor Display // Display the sensor data in GUI connect(sensorLowLevel, SIGNAL(signalLaserPlotRaw(LaserPlotData)), // raw data mainWindow, SLOT(slotLaserDisplay(LaserPlotData))); connect(sensorHighLevel, SIGNAL(signalSendLaserData(LaserPlotData)), // filtered data mainWindow, SLOT(slotLaserDisplay(LaserPlotData))); // Path Display // Display potential map and raw waypoints connect(pathPlanner, SIGNAL(pathDisplay(PathPlotData)), mainWindow, SLOT(updatePathDisplay(PathPlotData))); // Display calculated spline connect(actorHighLevel, SIGNAL(signalSplinePlot(PathPlotData)), mainWindow, SLOT(updatePathDisplay(PathPlotData))); // Display PID Plot connect(actorHighLevel, SIGNAL(signalPIDPlot(PIDPlotData)), mainWindow, SLOT(slotPIDPlot(PIDPlotData))); // Cam grabbed a frame, display in GUI connect(cam, SIGNAL(signalDisplayFrame(cv::Mat)), mainWindow, SLOT(slotDisplayFrame(cv::Mat))); // *********************************************************************** // emergency detected/received connect(sensorHighLevel,SIGNAL(signalEmergencyStopEnabled(bool)), // emergency stop durch kollisionsvermeidung actorLowLevel,SLOT(slotEmergencyStopEnabled(bool))); connect(gameEngine, SIGNAL(signalEmergencyStopEnabled(bool)), // stopMovement() von Referee actorLowLevel, SLOT(slotEmergencyStopEnabled(bool))); //send turn command from high sensor to low level actor connect(sensorHighLevel,SIGNAL(signalSendRobotControlParams(double,double)), actorLowLevel,SLOT(setRobotControllParams(double,double))); // controle command from High Level Aktor connect(actorHighLevel,SIGNAL(signalSendRobotControlParams(double,double)), actorLowLevel,SLOT(setRobotControllParams(double,double))); // Get waypoints from path planning into the path realizer connect(pathPlanner, SIGNAL(sendUpdatedWaypoints(QList< QPair<double,double> >)), actorHighLevel, SLOT(slotUpdateWaypoints(QList< QPair<double,double> >))); // SENSOR DATA TO HIGH LEVEL SENSOR connect(sensorLowLevel,SIGNAL(laserDataReady(QVector<double>)), sensorHighLevel, SLOT(getLaserData(QVector<double>))); connect(sensorLowLevel,SIGNAL(sonarDataReady(QVector<double>)), sensorHighLevel, SLOT(getSonarData(QVector<double>))); // start color detection from sensor connect(sensorHighLevel, SIGNAL(signalStartColorDetection()), cam, SLOT(slotStartColorDetection())); // color succesfully detected connect(cam, SIGNAL(signalColorDetected(CamColor)), sensorHighLevel, SLOT(slotColorDetected(CamColor))); // *********************************************************************** // update Odometry connect(sensorLowLevel,SIGNAL(updateOdometry()), actorLowLevel, SLOT(getOdometry())); connect(sensorHighLevel, SIGNAL(sendOdometryData(Position)), actorLowLevel, SLOT(setOdometry(Position))); // *********************************************************************** // Signals from GameEngine connect(gameEngine,SIGNAL(signalStartDetection(bool)), sensorHighLevel, SLOT(slotStartDetection(bool))); // Signals to GameEngine connect(sensorHighLevel, SIGNAL(signalSendTeamColor(CamColor)), gameEngine, SLOT(slotDetectionFinished(CamColor))); // starting the eventloop of all threads threadRobotLowLevel.start(); threadSensorHighLevel.start(); threadPathRealizer.start(); threadPathPlanner.start(); threadCam.start(); threadGameEngine.start(); // Starte Game Engine State Machine gameEngine->start(); // Start the PathPlanning "loop" /// \todo: Das ist erstmal nur zum Debuggen drinnen. // Spaeter darf die Pfadplanung nur bei beginn der Spielphase aus der GameEngine heraus angestossen werden QMetaObject::invokeMethod(pathPlanner, "planPath"); }