void WallFollowAlgorithm(void) { vector Z(0,0); int dist1, dist2, dist, Tdist1=50, Tdist2=50; int t=5; // tolerance Drive(136,136,50); // initially drive straight while ( SmartWait(2)==0 ) { dist1 = Sensor_To_Range(Sensor(1)); dist2 = Sensor_To_Range(Sensor(2)); if (dist1 > (dist2+t)) Drive(136-4,136-4,50); // dist1 > dist2 -> turn left else if (dist1 < (dist2-t)) Drive(136+5,136+5,50); // dist2 > dist1 -> turn right else if ( (dist1 > (Tdist1+t/2)) && (dist2 > (Tdist2+t/2)) ) // too far // dist1 > Tdist1 and dist2 > Tdist2 -> move close Drive(136+5,136-4,50); else if ( (dist1 < (Tdist1-t/2)) && (dist2 < (Tdist2-t/2)) ) // too close // dist1 < Tdist1 and dist2 < Tdist2 -> move far Drive(136-4,136+5,50); else Drive(136,136,50); } // Drive straight StopRobot(); }
ssize_t QuickCamDevice::ReadIIC8(uint8 address, uint8 *data) { status_t err; uint8 buffer[0x23]; memset(buffer, 0, sizeof(buffer)); buffer[0x20] = Sensor() ? Sensor()->IICReadAddress() : 0; buffer[0x21] = 1 - 1; buffer[0x22] = 0x03; buffer[0] = address; err = SendCommand(USB_REQTYPE_DEVICE_OUT, 0x04, STV_I2C_WRITE, 0, 0x23, buffer); PRINT((CH ": SendCommand: %s" CT, strerror(err))); if (err < B_OK) return err; buffer[0] = 0xaa; err = SendCommand(USB_REQTYPE_DEVICE_IN, 0x04, STV_I2C_READ, 0, 0x1, buffer); PRINT((CH ": SendCommand: %s" CT, strerror(err))); if (err < B_OK) return err; *data = buffer[0]; PRINT((CH ": 0x%02x" CT, *data)); return 1; }
void Mobile::initSensors() { /*Two food sensors, left and right*/ foodSensors[0] = Sensor( vec2(0.7,0.7), atan2(1,2), /*sens*/1, /*r*/FOOD_SENSOR_RADIUS ); foodSensors[1] = Sensor( vec2(-0.7, 0.7), atan2(-1,2), /*sens*/1, /*r*/FOOD_SENSOR_RADIUS ); }
SensorFusion::SensorFusion() : mSensorDevice(SensorDevice::getInstance()), mEnabled(false), mGyroTime(0) { sensor_t const* list; Sensor uncalibratedGyro; ssize_t count = mSensorDevice.getSensorList(&list); if (count > 0) { for (size_t i=0 ; i<size_t(count) ; i++) { if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { mAcc = Sensor(list + i); } if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) { mMag = Sensor(list + i); } if (list[i].type == SENSOR_TYPE_GYROSCOPE) { mGyro = Sensor(list + i); } if (list[i].type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { uncalibratedGyro = Sensor(list + i); } } // Use the uncalibrated gyroscope for sensor fusion when available if (uncalibratedGyro.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { mGyro = uncalibratedGyro; } // 200 Hz for gyro events is a good compromise between precision // and power/cpu usage. mEstimatedGyroRate = 200; mTargetDelayNs = 1000000000LL/mEstimatedGyroRate; mFusion.init(); } }
ssize_t QuickCamDevice::ReadIIC16(uint8 address, uint16 *data) { status_t err; uint8 buffer[0x23]; memset(buffer, 0, sizeof(buffer)); buffer[0x20] = Sensor() ? Sensor()->IICReadAddress() : 0; buffer[0x21] = 1 - 1; buffer[0x22] = 0x03; buffer[0] = address; err = SendCommand(USB_REQTYPE_DEVICE_OUT, 0x04, STV_I2C_WRITE, 0, 0x23, buffer); if (err < B_OK) return err; buffer[0] = 0xaa; buffer[1] = 0xaa; err = SendCommand(USB_REQTYPE_DEVICE_IN, 0x04, STV_I2C_READ, 0, 0x2, buffer); PRINT((CH ": SendCommand: %s" CT, strerror(err))); if (err < B_OK) return err; if (fChipIsBigEndian) *data = B_HOST_TO_BENDIAN_INT16(*(uint16 *)(&buffer[0])); else *data = B_HOST_TO_LENDIAN_INT16(*(uint16 *)(&buffer[0])); PRINT((CH ": 0x%04x" CT, *data)); return 2; }
SensorFusion::SensorFusion() : mSensorDevice(SensorDevice::getInstance()), mEnabled(false), mGyroTime(0) { sensor_t const* list; ssize_t count = mSensorDevice.getSensorList(&list); if (count > 0) { for (size_t i=0 ; i<size_t(count) ; i++) { if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { mAcc = Sensor(list + i); } if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) { mMag = Sensor(list + i); } if (list[i].type == SENSOR_TYPE_GYROSCOPE) { mGyro = Sensor(list + i); // 200 Hz for gyro events is a good compromise between precision // and power/cpu usage. mGyroRate = 200; mTargetDelayNs = 1000000000LL/mGyroRate; } } mFusion.init(); } }
void Adafruit_LSM9DS0::initI2C( int32_t sensorID ) { _i2c = true; _lsm9dso_sensorid_accel = sensorID + 1; _lsm9dso_sensorid_gyro = sensorID + 3; _accelSensor = Sensor(this, &Adafruit_LSM9DS0::readAccel, &Adafruit_LSM9DS0::getAccelEvent, &Adafruit_LSM9DS0::getAccelSensor); //_magSensor = Sensor(this, &Adafruit_LSM9DS0::readMag, &Adafruit_LSM9DS0::getMagEvent, &Adafruit_LSM9DS0::getMagSensor); _gyroSensor = Sensor(this, &Adafruit_LSM9DS0::readGyro, &Adafruit_LSM9DS0::getGyroEvent, &Adafruit_LSM9DS0::getGyroSensor); }
// default Adafruit_LSM9DS0::Adafruit_LSM9DS0( int32_t sensorID ) { _i2c = true; _lsm9dso_sensorid_accel = sensorID + 1; _lsm9dso_sensorid_mag = sensorID + 2; _lsm9dso_sensorid_gyro = sensorID + 3; _lsm9dso_sensorid_temp = sensorID + 4; _accelSensor = Sensor(this, &Adafruit_LSM9DS0::readAccel, &Adafruit_LSM9DS0::getAccelEvent, &Adafruit_LSM9DS0::getAccelSensor); _magSensor = Sensor(this, &Adafruit_LSM9DS0::readMag, &Adafruit_LSM9DS0::getMagEvent, &Adafruit_LSM9DS0::getMagSensor); _gyroSensor = Sensor(this, &Adafruit_LSM9DS0::readGyro, &Adafruit_LSM9DS0::getGyroEvent, &Adafruit_LSM9DS0::getGyroSensor); _tempSensor = Sensor(this, &Adafruit_LSM9DS0::readTemp, &Adafruit_LSM9DS0::getTempEvent, &Adafruit_LSM9DS0::getTempSensor); }
Adafruit_LSM9DS0::Adafruit_LSM9DS0(int8_t xmcs, int8_t gcs, int32_t sensorID ) { _i2c = false; // hardware SPI! _csg = gcs; _csxm = xmcs; _mosi = _miso = _clk = -1; _lsm9dso_sensorid_accel = sensorID + 1; _lsm9dso_sensorid_mag = sensorID + 2; _lsm9dso_sensorid_gyro = sensorID + 3; _lsm9dso_sensorid_temp = sensorID + 4; _accelSensor = Sensor(this, &Adafruit_LSM9DS0::readAccel, &Adafruit_LSM9DS0::getAccelEvent, &Adafruit_LSM9DS0::getAccelSensor); _magSensor = Sensor(this, &Adafruit_LSM9DS0::readMag, &Adafruit_LSM9DS0::getMagEvent, &Adafruit_LSM9DS0::getMagSensor); _gyroSensor = Sensor(this, &Adafruit_LSM9DS0::readGyro, &Adafruit_LSM9DS0::getGyroEvent, &Adafruit_LSM9DS0::getGyroSensor); _tempSensor = Sensor(this, &Adafruit_LSM9DS0::readTemp, &Adafruit_LSM9DS0::getTempEvent, &Adafruit_LSM9DS0::getTempSensor); }
void PenFollowAlgorithm(void) { unsigned long start_time; int pen_dist=30, dist, detected = False, lost = False, infi = 60, played = False; // put pen facing wheel #1 // turn robot to the right until it finds the center of the pen Turn(Right, Slow); DisplayTxt("Scanning..."); do { dist = Sensor_To_Range(Sensor(3)); if (dist <= pen_dist) detected = True; } while ( (detected == False) && (SmartWait(1)==0) ); StopRobot(); // stop robot // begin tracking loop do { // ADJUST ANGULAR POSITION dist = Sensor_To_Range(Sensor(3)); if (dist > infi) { // target lost turn left/right played = False; Turn(Right,Slow); Wait(50); StopRobot(); // turn right for awhile dist = Sensor_To_Range(Sensor(3)); if (dist > infi) { // target is not to the right, Turn(Left,Slow);// turn left start_time = 0; do { dist = Sensor_To_Range(Sensor(3)); } while ((start_time++ < 1000) && (dist > infi) && (SmartWait(1)==0) ); // turn until found dist = Sensor_To_Range(Sensor(3)); if (dist > infi) { lost = True; break; } StopRobot(); } } dist = Sensor_To_Range(Sensor(3)); // ADJUST LINEAR POSITION if ( (dist < infi) && (dist < (pen_dist-5)) ) { // too close to target played = False; Move(RToServo3); do { dist = Sensor_To_Range(Sensor(3)); if (dist > infi) // lost angular tracking break; } while (dist < pen_dist); // get dist to pen_dist StopRobot(); } if ( (dist < infi) && (dist > (pen_dist+5)) ) { // too far played = False; Move(ToServo3); do { dist = Sensor_To_Range(Sensor(3)); if (dist > infi) // lost angular tracking break; } while (dist > pen_dist); // get dist to pen_dist StopRobot(); } if ( (dist < (pen_dist+5)) && (dist > (pen_dist-5)) && (played == False) ) { printf("Yep\n"); played = True; } } while ( (lost == False) && (SmartWait(10)==0) ); StopRobot(); }
task robot() { while(true) { if(Sensor(COLOR_SENSOR) < 3) { backward(); Wait(300); rotate(); Wait(300); } else { if(SensorUS(ULTRA_SENSOR) < DISTANCE) // Trying to find an object { forward(); Wait(100); } else { rotate(); } Wait(15); } } }
Alarm::Alarm() { sensor = Sensor(); lowPressureTreshold = 17; highPressureTreshold = 21; alarmOn = false; }
MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent), ui(new Ui::MainWindow) { ui->setupUi(this); _sensors.push_back(Sensor(SensorData('s', CUSTOM_SENSOR01, 0, 0), 25.5)); _sensors.push_back(Sensor(SensorData('s', CUSTOM_SENSOR02, 0, 0), 27.5)); _sensors.push_back(Sensor(SensorData('s', CUSTOM_SENSOR03, 0, 0), 28.5)); for (Sensor sensor: _sensors) { ui->comboBox_SensorId->addItem(QString::number(sensor.sensorData.byte01), sensor.sensorData.byte01); _sensorIds.push_back(sensor.sensorData.byte01); } _tcpClient = new TcpClient(CUSTOM_IPV4ADDRESS, CUSTOM_PORT, _sensorIds, CUSTOM_QUERYINTERVAL, this); bool isOk; // TcpClient: Connections isOk = connect(_tcpClient, SIGNAL(dataReceived(quint8, qreal)), this, SLOT(onDataReceived(quint8, qreal))); Q_ASSERT(isOk); Q_UNUSED(isOk); isOk = connect(_tcpClient, SIGNAL(dataReceived(SensorData)), this, SLOT(onDataReceived(SensorData))); Q_ASSERT(isOk); Q_UNUSED(isOk); // GUI: Connections isOk = connect(ui->pushButton_TemperatureSet, SIGNAL(clicked()), this, SLOT(setTemperatureDesired())); Q_ASSERT(isOk); Q_UNUSED(isOk); isOk = connect(ui->comboBox_SensorId, SIGNAL(currentIndexChanged(int)), this, SLOT(on_comboBox_SensorId_currentIndexChanged(int))); Q_ASSERT(isOk); Q_UNUSED(isOk); _tcpClient->start(); }
ssize_t QuickCamDevice::WriteIIC(uint8 address, uint8 *data, size_t count) { int i; uint8 buffer[0x23]; if (count > 16) return EINVAL; memset(buffer, 0, sizeof(buffer)); buffer[0x20] = Sensor() ? Sensor()->IICWriteAddress() : 0; buffer[0x21] = count - 1; buffer[0x22] = 0x01; for (i = 0; i < count; i++) { buffer[i] = address + i; buffer[i+16] = data[i]; } return SendCommand(USB_REQTYPE_DEVICE_OUT, 0x04, STV_I2C_WRITE, 0, 0x23, buffer); }
//######################################################################################################################################################################33 void _int_(_TIMER_4_VECTOR) isr_t22(void) { Sensor(); printf("puta Cheguei\n"); IFS0bits.T4IF = 0; }
void GPhidgetInterfaceKitModule::TriggeredSensorValueChanged( int indexSensor, int theValue ) { // // you can querry the 12 bit value ! But on Interface kit 1018, it is only 10 bit so it doesn't make sense to use it, does it? // int the12BitRawValue; // PhidgetInterfaceKit_getSensorRawValue(m_ThePhidgetInterfaceKit, indexSensor, &the12BitRawValue); GPhidgetSensor* pTheSensor = Sensor(indexSensor); if(pTheSensor) pTheSensor->ModuleTriggeredValueChanged(theValue); }
void simple_test(void) { int s1, s2, s3; Drive(0, 0, 0); /* Motors off */ printf("Read sensors\n"); while (1) { s1 = Sensor(1); s2 = Sensor(2); s3 = Sensor(3); printf("1=%d(%d) 2=%d(%d) 3=%d(%d)\n", s1, Sensor_To_Range(s1), s2, Sensor_To_Range(s2), s3, Sensor_To_Range(s3)); sleep(1); } }
CorrectedGyroSensor::CorrectedGyroSensor(sensor_t const* list, size_t count) : mSensorDevice(SensorDevice::getInstance()), mSensorFusion(SensorFusion::getInstance()) { for (size_t i=0 ; i<count ; i++) { if (list[i].type == SENSOR_TYPE_GYROSCOPE) { mGyro = Sensor(list + i); break; } } }
status_t QuickCamDevice::StartTransfer() { SetScale(1); if (Sensor()) SetVideoFrame(BRect(0, 0, Sensor()->MaxWidth()-1, Sensor()->MaxHeight()-1)); //SetVideoFrame(BRect(0, 0, 320-1, 240-1)); DumpRegs(); #if 0 err = ReadReg(SN9C102_CHIP_CTRL, &r, 1, true); if (err < 0) return err; r |= 0x04; err = WriteReg8(SN9C102_CHIP_CTRL, r); if (err < 0) return err; #endif return CamDevice::StartTransfer(); }
void TravelerAlgorithm(void) { vector Z(0,0); double omega, vel=0.07; // vel is top speed of servos int chosen_dir = 1, quit_loop=0; int sensor[6], sensor_obstacle=30, sensor_clearview=40; Move(chosen_dir); do { do { // drive until robot sees an obstacle using a current sensor quit_loop = SmartWait(1); // obstacle detected } while ( (Sensor_To_Range(Sensor(chosen_dir)) > sensor_obstacle) && (quit_loop == 0) && (chosen_dir!=0) ); sensor[1] = Sensor_To_Range(Sensor(1)); // get sensors sensor[2] = Sensor_To_Range(Sensor(2)); sensor[3] = Sensor_To_Range(Sensor(3)); sensor[4] = sensor[1]; sensor[5] = sensor[2]; // sensor+1 -> obstalce & sensor+2 -> obstalce if ( (sensor[chosen_dir+2] < sensor_obstacle) && (sensor[chosen_dir+1] < sensor_obstacle) ) { Vector_Drive(Z, 0.500); chosen_dir=0; } else { // sensor+1 -> clear & sensor+2 -> obstacle if ( (sensor[chosen_dir+1] > sensor_clearview) && (sensor[chosen_dir+2] < sensor_obstacle) ) chosen_dir+=1; // sensor+1 -> obstalce & sensor+2 -> clear if ( (sensor[chosen_dir+1] < sensor_obstacle) && (sensor[chosen_dir+2] > sensor_clearview) ) chosen_dir+=2; // sensor+1 -> clear & sensor+2 -> clear if ( (sensor[chosen_dir+2] > sensor_clearview) && (sensor[chosen_dir+1] > sensor_clearview) ) // pick 2 for now, change to random later chosen_dir+=2; if ((chosen_dir) > 3) chosen_dir-=3; // prevent overflow Move(chosen_dir); } Display(chosen_dir); } while (quit_loop==0); StopRobot(); }
RotationVectorSensor2::RotationVectorSensor2() : mSensorDevice(SensorDevice::getInstance()), mEnabled(false), mHasData(false) { sensor_t const* list; ssize_t count = mSensorDevice.getSensorList(&list); if (count > 0) { for (size_t i=0 ; i<size_t(count) ; i++) { if (list[i].type == SENSOR_TYPE_ORIENTATION) { mOrientation = Sensor(list + i); } } } }
/*! @brief Default constructor for NUSensorsData */ NUSensorsData::NUSensorsData() : NUData(), TimestampedData() { #if DEBUG_NUSENSORS_VERBOSITY > 0 debug << "NUSensorsData::NUSensorsData" << endl; #endif CurrentTime = 0; PreviousTime = 0; // If this has already been initialised, don't do it again or bad stuff will happen. if(m_ids.size() < m_num_sensor_ids) m_ids.insert(m_ids.begin(), NUData::m_common_ids.begin(), NUData::m_common_ids.end()); m_ids_copy = m_ids; m_id_to_indices = vector<vector<int> >(m_ids.size(), vector<int>()); for (size_t i=0; i<m_ids.size(); i++) m_sensors.push_back(Sensor(m_ids[i]->Name)); }
// Default constructor for AOI, Initializes Mission List and Sensor list as well AOI::AOI(int num_sensor_input, int num_mission_input, int mission_duration, int mission_req_sensors) { // initialize AOI specific variables aoi_size = AOI_SIZE; num_sensor = num_sensor_input; num_mission = num_mission_input; // initialize Sensors for (int i = 0; i < num_sensor; i++) { s_list.push_back(Sensor(i, aoi_size)); } // initialize Missions for (int i = 0; i < num_mission; i++) { m_list.push_back(Mission(aoi_size, mission_duration, mission_req_sensors)); } }
void Simulator::runThreadOnHouse(int houseIndex) { string houseFileName = houseFileNames[houseIndex]; // read house file and run all algorithms on the specific house unique_ptr<House> house = make_unique<House>(); int handle = readHouseFile(houseIndex, houseFileName, houseErrors.get(), house.get()); // pass ptr without releasing (house will be updated) if (handle) { // error isValidHouses[houseIndex] = false; // (default is false anyway) return; } numOfWorkingHouses++; // atomic increase isValidHouses[houseIndex] = true; // automatic win for each algorithm if the dirt in the house == 0 if (house->sumOfDirt == 0) { map<string, int> autoWinScore; autoWinScore["simulation_steps"] = 0; autoWinScore["winner_num_steps"] = 0; autoWinScore["this_num_steps"] = 0; autoWinScore["sum_dirt_in_house"] = 0; autoWinScore["dirt_collected"] = 0; autoWinScore["is_back_in_docking"] = 1; autoWinScore["actual_position_in_competition"] = 1; auto nameIterator = registrar.getAlgorithmNames().begin(); for (int i = 0; i < numOfAlgorithms; i++) { if (score_function != NULL) { scores[*nameIterator][houseIndex] = (*score_function)(autoWinScore); } else { scores[*nameIterator][houseIndex] = score(autoWinScore); } } return; // that's all } // new instance of all algorithms AlgorithmRegistrar& curRegistrar = AlgorithmRegistrar::getInstance(); auto algorithms = curRegistrar.getAlgorithms(); auto& algorithmNames = curRegistrar.getAlgorithmNames(); auto nameIterator = algorithmNames.begin(); vector<bool> if_end(numOfAlgorithms); vector<bool> into_wall(numOfAlgorithms); vector<int> curBattery(numOfAlgorithms); vector<int> numSteps(numOfAlgorithms); vector<int> positionInComp(numOfAlgorithms, 10); // default position is 10 vector<Direction> lastMoves(numOfAlgorithms, Direction::Stay); int simulation_num_steps = 0; int max_steps = house->maxSteps; int batteryCapacity = (config.find("BatteryCapacity"))->second; int batteryConsumptionRate = (config.find("BatteryConsumptionRate"))->second; int batteryRechargeRate = (config.find("BatteryRechargeRate"))->second; for (int i = 0; i < numOfAlgorithms; i++) curBattery[i] = batteryCapacity; bool is_winner = false; int winner_num_steps = -1; int cur_stage_winners = 0; int cur_position = 1; int finished = 0; bool already_alerted_more_steps = false; int algIndex; // make a copy of the current house for every algorithm and assign a sensor to it unique_ptr<House[]> curHouses = make_unique<House[]>(numOfAlgorithms); vector<Sensor> sensors; for (int l = 0; l < numOfAlgorithms; l++) { copyHouse(curHouses[l], house.get()); sensors.emplace_back(Sensor(&curHouses[l])); } int i = 0; for (auto& algorithm : algorithms) { algorithm->setSensor(sensors[i]); algorithm->setConfiguration(config); i++; } if (DEBUG) { // print the house with D (no R) - for debugging string space = " "; if (house->rows > 60 || house->cols > 40) space = ""; if (house->matrix != NULL) { for (int i = 0; i < house->rows; i++) { for (int j = 0; j < house->cols; j++) { cout << house->matrix[i][j] << space; } cout << endl; } } // print with robot R (no D) cout << endl; printHouseWithRobot(curHouses[0]); // house == curHouses[0] == ... == curHouses[numOfAlgorithms-1] } while (true) { simulation_num_steps++; if (SHOW_SIMULATION_HOUSES) { getchar(); cout << "Step " << simulation_num_steps << endl; } // simulate one step for each algorithm algIndex = -1; nameIterator = algorithmNames.begin(); for (auto& algorithm : algorithms) { // increase for the next algorithm algIndex++; if (if_end[algIndex] == true) { nameIterator++; continue; } // pass last move of the algorithm and update the new one Direction direction = algorithm->step(lastMoves[algIndex]); lastMoves[algIndex] = direction; // if leaving docking station -> load battery if (curHouses[algIndex].matrix[curHouses[algIndex].robot.row][curHouses[algIndex].robot.col] == 'D') { curBattery[algIndex] = MIN(batteryCapacity, curBattery[algIndex] + batteryRechargeRate); } // consume battery only if did not start the move from the docking station // staying or starting the move from the docking station does not consume battery if (curHouses[algIndex].robot != curHouses[algIndex].docking) curBattery[algIndex] -= batteryConsumptionRate; // make the step on the current house of the algorithm switch (direction) { case static_cast<Direction>(0) : curHouses[algIndex].robot.col++; break; case static_cast<Direction>(1) : curHouses[algIndex].robot.col--; break; case static_cast<Direction>(2) : curHouses[algIndex].robot.row++; break; case static_cast<Direction>(3) : curHouses[algIndex].robot.row--; break; default: break; // do nothing for 'Stay' } //cleaning dust when entering a cell if (curHouses[algIndex].matrix[curHouses[algIndex].robot.row][curHouses[algIndex].robot.col] > '0' && curHouses[algIndex].matrix[curHouses[algIndex].robot.row][curHouses[algIndex].robot.col] <= '9') { curHouses[algIndex].matrix[curHouses[algIndex].robot.row][curHouses[algIndex].robot.col] = curHouses[algIndex].matrix[curHouses[algIndex].robot.row][curHouses[algIndex].robot.col] - 1; curHouses[algIndex].sumOfDirt--; } // create a snapshot of the current house if desired && there was no previous error with creating a folder for the current alg+home // note that we do take a snapshot of the case in which a robot gets into a wall (that's ok as amir said) if (isFlagVideoUp && !curHouses[algIndex].folderError) { curHouses[algIndex].montage(*nameIterator, videoErrors); } // walked into a wall -> stop the algorithm from running. its score will be zero if (curHouses[algIndex].matrix[curHouses[algIndex].robot.row][curHouses[algIndex].robot.col] == 'W') { into_wall[algIndex] = true; if_end[algIndex] = true; finished++; // make the error note to be printed later (at the end after all other errors) int index = static_cast<int>((*nameIterator).find(".so")); string name = (*nameIterator).substr(0, index); string wallError = "Algorithm "; wallError += name; wallError += " when running on House "; index = static_cast<int>(curHouses[algIndex].houseFileName.find_last_of('.')); name = (curHouses[algIndex].houseFileName).substr(0, index); name = name.substr(6, name.size() - 6); wallError += name; wallError += " went on a wall in step "; wallError += to_string(simulation_num_steps); walkingIntoWallsErrors[houseIndex] = wallError; algorithmIntoWall = true; if (DEBUG) cout << INTO_WALL << endl; nameIterator++; continue; } // for debug purpose if (SHOW_SIMULATION_HOUSES) { cout << "Robot(" << (*nameIterator) << ") Battery: " << curBattery[algIndex] << endl; printHouseWithRobot(curHouses[algIndex]); } if (curHouses[algIndex].sumOfDirt == 0 && curHouses[algIndex].robot == curHouses[algIndex].docking) { if (DEBUG) cout << "Robot wins (cleaned the whole house in the limited time)." << endl; // for debug purpose if_end[algIndex] = true; cur_stage_winners++; if (!is_winner) { is_winner = true; winner_num_steps = simulation_num_steps; max_steps = MIN(max_steps, simulation_num_steps + config["MaxStepsAfterWinner"]); } finished++; positionInComp[algIndex] = cur_position; numSteps[algIndex] = simulation_num_steps; nameIterator++; continue; } if (curBattery[algIndex] <= 0) { if (DEBUG) cout << BATTERY_DEAD << endl; // for debug purpose if_end[algIndex] = true; finished++; nameIterator++; continue; } nameIterator++; } // finished one step of each algorithm if (cur_stage_winners > 0) cur_position = cur_position + cur_stage_winners; cur_stage_winners = 0; // - MAX-STEPS-AFTER-WINNNER ALERT - if (!already_alerted_more_steps) { // let all the other algorithms (that did not win in the this last move) know 'MaxStepsAfterWinner' // alret them only at the first round when some algorithm wins // the condition is true ONLY on the first round when some algorithm wins if (winner_num_steps == simulation_num_steps) { // if someone wins, max_steps is already updated in the loop, so it's simply a subtraction int alert_more_steps = max_steps - simulation_num_steps; algIndex = 0; for (auto& algorithm : algorithms) { // alert only algorithms that did not win / die (they're battery finished before charging) if (if_end[algIndex] == false) { algorithm->aboutToFinish(alert_more_steps); } algIndex++; } already_alerted_more_steps = true; if (DEBUG) cout << endl << "ALERT TO ALL ALGORITHMS: more steps = " << alert_more_steps << endl; } // other case -> no one won but there are 'maxstepsafterwinner' more steps till the end // alert all algorithms else if (!is_winner && ((max_steps - simulation_num_steps) == config["MaxStepsAfterWinner"])) { algIndex = 0; for (auto& algorithm : algorithms) { // alert only algorithms that did not die (they're battery finished before charging / went into a wall) if (if_end[algIndex] == false) { algorithm->aboutToFinish(config["MaxStepsAfterWinner"]); } algIndex++; } already_alerted_more_steps = true; if (DEBUG) cout << endl << "ALERT TO ALL ALGORITHMS: more steps = " << config["MaxStepsAfterWinner"] << endl; } } // end of game if (finished == numOfAlgorithms || simulation_num_steps == max_steps) { if (DEBUG) cout << NO_MORE_MOVES << endl; // for debug purpose for (algIndex = 0; algIndex < numOfAlgorithms; algIndex++) { // if didn't change -> he didn't win. set number of steps to simulation steps if (numSteps[algIndex] == 0) { numSteps[algIndex] = simulation_num_steps; } } break; } } // score the algorithms on the house // if none won -> winner num steps = simulation num steps // call the right score method (Score.h or calc_score from the loaded score_formula.so) // (call calc_score if score_loaded==true (field in Simulator.h) and call the score method in Score.h when it's false) if (winner_num_steps == -1) winner_num_steps = simulation_num_steps; nameIterator = algorithmNames.begin(); for (int algIndex = 0; algIndex < numOfAlgorithms; algIndex++) { int is_back_in_docking = (curHouses[algIndex].robot == curHouses[algIndex].docking) ? true : false; if (into_wall[algIndex] == true) // if walked into a wall, score=0 scores[*nameIterator][houseIndex] = 0; else { map<string, int> score_params; score_params["simulation_steps"] = simulation_num_steps; score_params["winner_num_steps"] = winner_num_steps; score_params["this_num_steps"] = numSteps[algIndex]; score_params["sum_dirt_in_house"] = curHouses[algIndex].initialSumOfDirt; score_params["dirt_collected"] = curHouses[algIndex].initialSumOfDirt - curHouses[algIndex].sumOfDirt; score_params["is_back_in_docking"] = is_back_in_docking; if (curHouses[algIndex].sumOfDirt == 0 && is_back_in_docking) { score_params["actual_position_in_competition"] = positionInComp[algIndex]; } else { score_params["actual_position_in_competition"] = 10; } if (score_function != NULL) { scores[*nameIterator][houseIndex] = (*score_function)(score_params); } else { scores[*nameIterator][houseIndex] = score(score_params); } if (scores[*nameIterator][houseIndex] == -1) isErrorInScoreCalc = true; } nameIterator++; } // create the videos for all the algorithms on the house if (isFlagVideoUp) { algIndex = 0; nameIterator = algorithmNames.begin(); string simulationDir; string imagesExpression; while (nameIterator != algorithmNames.end()) { // make a video only when the folder creation was successfull if (!curHouses[algIndex].folderError) { // image creation error if (curHouses[algIndex].imageErrors > 0) { string error_msg = "Error: In the simulation " + *nameIterator + ", " + curHouses[algIndex].houseFileName + ": the creation of " + to_string(curHouses[algIndex].imageErrors) + " images was failed"; videoErrors.push_back(error_msg); } // make a video only when at least one snapshot was created successfully if (curHouses[algIndex].createVideo) { simulationDir = "simulations/" + *nameIterator + "_" + curHouses[algIndex].houseFileName + "/"; imagesExpression = simulationDir + "image%5d.jpg"; if (Encoder::encode(imagesExpression, *nameIterator + "_" + curHouses[algIndex].houseFileName + ".mpg")) { // video creation fail string error_msg = "Error: In the simulation " + *nameIterator + ", " + curHouses[algIndex].houseFileName + ": video file creation failed"; videoErrors.push_back(error_msg); } } // remove the folder with all its content (images) removeDirectory(curHouses[algIndex].imagesDirPath); // optional: if removing folder fails -> add to errors /* if (removeDirectory(curHouses[algIndex].imagesDirPath)) { string error_msg = "Error: In the simulation " + *nameIterator + ", " + curHouses[algIndex].houseFileName + ": removing folder " + curHouses[algIndex].imagesDirPath + " failed"; videoErrors.push_back(error_msg); } */ } nameIterator++; algIndex++; } } if (DEBUG) getchar(); }
Sensor DataClass::sensor(String name, byte pin, Timer time) { return Sensor(name, pin, time); }
/** Sensor **/ Sensor DataClass::sensor(String name, byte pin) { return Sensor(name, pin, _timer); }
Car::Car() : carPosition(100, 100), carDirection(90), carSpeed(1) { sensorsList.push_back(Sensor(-(CAR_WIDTH / 2), -(CAR_LENGTH / 2 + 2 * SENSOR_RADIUS), SENSOR_RADIUS)); sensorsList.push_back(Sensor(CAR_WIDTH / 2, -(CAR_LENGTH / 2 + 2 * SENSOR_RADIUS), SENSOR_RADIUS)); }
Flowerbed::Flowerbed(unsigned int index) : _index(index), _tempSensor(Sensor(SensorType::temperature)), _lastWatering(-1 * WATERING_DELAY - MINUTE) { }
int main(int argc, char *argv[]) { try { cfg_opt_t opts_sensor[] = { CFG_INT(const_cast<char *>("crate"), 0xff, CFGF_NONE), CFG_INT(const_cast<char *>("fru"), 0xff, CFGF_NONE), CFG_STR(const_cast<char *>("card"), const_cast<char *>(""), CFGF_NONE), CFG_STR(const_cast<char *>("sensor"), const_cast<char *>(""), CFGF_NONE), CFG_END() }; cfg_opt_t opts[] = { CFG_SEC(const_cast<char *>("sensor"), opts_sensor, CFGF_MULTI), CFG_STR(const_cast<char *>("outfile"), const_cast<char *>("data.csv"), CFGF_NONE), CFG_STR(const_cast<char *>("host"), const_cast<char *>("localhost"), CFGF_NONE), CFG_STR(const_cast<char *>("password"), const_cast<char *>(""), CFGF_NONE), CFG_INT(const_cast<char *>("port"), 4681, CFGF_NONE), CFG_INT(const_cast<char *>("interval"), 5, CFGF_NONE), CFG_END() }; cfg_t *cfg = cfg_init(opts, CFGF_NONE); cfg_set_validate_func(cfg, "host", &cfg_validate_hostname); cfg_set_validate_func(cfg, "port", &cfg_validate_port); if (argc < 2) { printf("%s sensor-spec.conf\n", argv[0]); exit(1); } if (cfg_parse(cfg, argv[1]) == CFG_PARSE_ERROR) exit(1); const char *host = cfg_getstr(cfg, "host"); const char *pass = cfg_getstr(cfg, "password"); const char *outfile = cfg_getstr(cfg, "outfile"); uint16_t port = cfg_getint(cfg, "port"); uint32_t interval = cfg_getint(cfg, "interval"); sysmgr::sysmgr sm(host, pass, port); try { sm.connect(); } catch (sysmgr::sysmgr_exception &e) { printf("Unable to connect to system manager: %s\n", e.message.c_str()); exit(2); } std::vector<Sensor> sensors; std::vector<sysmgr::crate_info> sm_crates = sm.list_crates(); for (std::vector<sysmgr::crate_info>::iterator it = sm_crates.begin(); it != sm_crates.end(); it++) { if (!it->connected) printf("Warning: Crate %hhu is not connected at this time. Discarding all data related to it.\n", it->crateno); } for(unsigned int i = 0; i < cfg_size(cfg, "sensor"); i++) { cfg_t *cfgsensor = cfg_getnsec(cfg, "sensor", i); uint8_t crate = cfg_getint(cfgsensor, "crate"); uint8_t fru = cfg_getint(cfgsensor, "fru"); const char *card = cfg_getstr(cfgsensor, "card"); const char *sensor = cfg_getstr(cfgsensor, "sensor"); bool found = false; for (std::vector<sysmgr::crate_info>::iterator c_it = sm_crates.begin(); c_it != sm_crates.end(); c_it++) { if (!c_it->connected) continue; if (crate != 0xff && c_it->crateno != crate) continue; std::vector<sysmgr::card_info> sm_frus = sm.list_cards(c_it->crateno); for (std::vector<sysmgr::card_info>::iterator f_it = sm_frus.begin(); f_it != sm_frus.end(); f_it++) { if (fru != 0xff && f_it->fru != fru) continue; if (card[0] != '\0' && f_it->name != card) continue; std::vector<sysmgr::sensor_info> sm_sensors = sm.list_sensors(c_it->crateno, f_it->fru); for (std::vector<sysmgr::sensor_info>::iterator s_it = sm_sensors.begin(); s_it != sm_sensors.end(); s_it++) { if (s_it->type == 'E' || s_it->type == 'O') continue; // We don't know how to read these. if (sensor[0] != '\0' && s_it->name != sensor) continue; sensors.push_back(Sensor(c_it->crateno, f_it->fru, s_it->name, stdsprintf("C%hhu %s (%s) %s", c_it->crateno, sysmgr::sysmgr::get_slotstring(f_it->fru).c_str(), f_it->name.c_str(), s_it->name.c_str()))); found = true; } } } if (!found) printf("Warning: Unable to find a match for %hhu %hhu \"%s\" \"%s\"\n", crate, fru, card, sensor); } FILE *fd = fopen(outfile, "w"); if (!fd) { printf("Unable to open %s for writing\n", outfile); exit(1); } cfg_free(cfg); printf("Sensors indexed. Now polling %d sensors.\n", (int)sensors.size()); fprintf(fd, "Time"); for (std::vector<Sensor>::iterator it = sensors.begin(); it != sensors.end(); it++) { fprintf(fd, CSV_COMMA "\"%s\"", it->name.c_str()); } fprintf(fd, "\n"); fflush(fd); time_t now; char timestamp[32]; while (1) { time(&now); strftime(timestamp, 32, "%Y-%m-%d %H:%M:%S", localtime(&now)); fprintf(fd, "%s", timestamp); for (std::vector<Sensor>::iterator it = sensors.begin(); it != sensors.end(); it++) { fprintf(fd, CSV_COMMA); try { sysmgr::sensor_reading reading = sm.sensor_read(it->crate, it->fru, it->sensor); if (reading.threshold_set) fprintf(fd, "%f", reading.threshold); else fprintf(fd, "0x%04hx", reading.eventmask); } catch (sysmgr::sysmgr_exception &e) { //printf("Error reading sensor \"%s\": %s\n", it->name.c_str(), e.message.c_str()); } //usleep(100000); } fprintf(fd, "\n"); fflush(fd); sleep(interval); if (!sm.connected()) break; } } catch (sysmgr::sysmgr_exception &e) { printf("Caught fatal exception: %s\n", e.message.c_str()); printf("Goodbye.\n"); } }
//## class Sensor Sensor::Sensor(IOxfActive* theActiveContext) : limit(0) { NOTIFY_REACTIVE_CONSTRUCTOR(Sensor, Sensor(), 0, Default_Sensor_Sensor_SERIALIZE); setActiveContext(theActiveContext, false); door = NULL; initStatechart(); }