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();
}
