//------------------------------------------------------------------------------ int main(int argc, char *argv[]) { GMainLoop *loop; GError * err = NULL; // Sensors Sensors s; s.scan(); // Bluetooth scan // Bluetooth b; b.scan(); #if 0 GIOChannel *channel = g_io_channel_unix_new(b.Handle()); g_io_add_watch(channel, (GIOCondition)(G_IO_IN | G_IO_OUT | G_IO_ERR | G_IO_HUP), onEvent, channel); #endif //b.close(); // socket server // GSocketService * server = g_socket_service_new(); g_socket_listener_add_inet_port((GSocketListener*)server, PORT, NULL, &err); if (err != NULL) { g_error("ERROR: %s", err->message); } g_signal_connect (server, "incoming", G_CALLBACK(onConnection), NULL); g_socket_service_start(server); // mainloop and timer // loop = g_main_loop_new ( NULL , FALSE ); g_timeout_add (5000 , OnTimer , loop); g_main_loop_run (loop); g_main_loop_unref(loop); return 0; }
// MEG patches positions are reported line by line in the positions Matrix (same for positions) // mat is supposed to be filled with zeros // mat is the linear application which maps x (the unknown vector in symmetric system) -> binf (contrib to MEG response) void assemble_SurfSource2MEG(Matrix& mat, const Mesh& sources_mesh, const Sensors& sensors) { Matrix positions = sensors.getPositions(); Matrix orientations = sensors.getOrientations(); const unsigned nsquids = positions.nlin(); mat = Matrix(nsquids, sources_mesh.nb_vertices()); mat.set(0.0); for ( unsigned i = 0; i < nsquids; ++i) { PROGRESSBAR(i, nsquids); Vect3 p(positions(i, 0), positions(i, 1), positions(i, 2)); Matrix FergusonMat(3, mat.ncol()); FergusonMat.set(0.0); operatorFerguson(p, sources_mesh, FergusonMat, 0, 1.); for ( unsigned j = 0; j < mat.ncol(); ++j) { Vect3 fergusonField(FergusonMat(0, j), FergusonMat(1, j), FergusonMat(2, j)); Vect3 normalizedDirection(orientations(i, 0), orientations(i, 1), orientations(i, 2)); normalizedDirection.normalize(); mat(i, j) = fergusonField * normalizedDirection; } } mat = sensors.getWeightsMatrix() * mat; // Apply weights }
// MEG patches positions are reported line by line in the positions Matrix (same for positions) // mat is supposed to be filled with zeros // mat is the linear application which maps x (the unknown vector in symmetric system) -> bFerguson (contrib to MEG response) void assemble_Head2MEG(Matrix& mat, const Geometry& geo, const Sensors& sensors) { Matrix positions = sensors.getPositions(); Matrix orientations = sensors.getOrientations(); const unsigned nbIntegrationPoints = sensors.getNumberOfPositions(); unsigned p0_p1_size = (geo.size() - geo.outermost_interface().nb_triangles()); Matrix FergusonMat(3*nbIntegrationPoints, geo.nb_vertices()); assemble_ferguson(geo, FergusonMat, positions); mat = Matrix(nbIntegrationPoints, p0_p1_size); mat.set(0.0); for ( unsigned i = 0; i < nbIntegrationPoints; ++i) { PROGRESSBAR(i, nbIntegrationPoints); for ( Vertices::const_iterator vit = geo.vertex_begin(); vit != geo.vertex_end(); ++vit) { Vect3 fergusonField(FergusonMat(3*i, vit->index()), FergusonMat(3*i+1, vit->index()), FergusonMat(3*i+2, vit->index())); Vect3 normalizedDirection(orientations(i, 0), orientations(i, 1), orientations(i, 2)); normalizedDirection.normalize(); mat(i, vit->index()) = fergusonField * normalizedDirection; } } mat = sensors.getWeightsMatrix() * mat; // Apply weights }
// Creates the DipSource2MEG Matrix with unconstrained orientations for the sources. // MEG patches positions are reported line by line in the positions Matrix (same for positions) // mat is supposed to be filled with zeros // sources is the name of a file containing the description of the sources - one dipole per line: x1 x2 x3 n1 n2 n3, x being the position and n the orientation. void assemble_DipSource2MEG(Matrix& mat, const Matrix& dipoles, const Sensors& sensors) { Matrix positions = sensors.getPositions(); Matrix orientations = sensors.getOrientations(); if ( dipoles.ncol() != 6) { std::cerr << "Dipoles File Format Error" << std::endl; exit(1); } // this Matrix will contain the field generated at the location of the i-th squid by the j-th source mat = Matrix(positions.nlin(), dipoles.nlin()); // the following routine is the equivalent of operatorFerguson for pointlike dipoles. for ( unsigned i = 0; i < mat.nlin(); ++i) { for ( unsigned j = 0; j < mat.ncol(); ++j) { Vect3 r(dipoles(j, 0), dipoles(j, 1), dipoles(j, 2)); Vect3 q(dipoles(j, 3), dipoles(j, 4), dipoles(j,5)); Vect3 diff(positions(i, 0), positions(i, 1), positions(i, 2)); diff -= r; double norm_diff = diff.norm(); Vect3 fergusonField = q ^ diff / (norm_diff * norm_diff * norm_diff); Vect3 normalizedDirection(orientations(i, 0), orientations(i, 1), orientations(i, 2)); normalizedDirection.normalize(); mat(i, j) = fergusonField * normalizedDirection * MU0 / (4.0 * M_PI); } } mat = sensors.getWeightsMatrix() * mat; // Apply weights }
virtual std::vector<TBS::BB::WebUI::SensorInfo> GetSensors() { Poco::Mutex::ScopedLock l(mtx); std::vector<TBS::BB::WebUI::SensorInfo> all; for (Sensors::iterator i = sensors.begin(); i != sensors.end(); i++) { all.push_back(i->second); } return all; }
Input_Out Run(Input_In input) { //Structs and stuff Input_Out output; X360Controller_In xbIn; X360Controller_out xbOut; Sensors_In sensIn; Sensors_Out sensOut; sensOut = sensors->Run(sensIn); xbOut = controller->Run(xbIn); //Retreving controller info output.returnX = xbOut.returnX; output.returnY = xbOut.returnY; output.returnRotation = xbOut.returnRotation; output.returnTurbo = xbOut.returnTurbo; output.returnTriggerPressed = xbOut.returnTriggerPressed; output.returnButtonPressed = xbOut.returnButtonPressed; //Retreving sensor info output.returnGyroAngle = sensOut.returnGyroAngle; output.returnAccelX = sensOut.returnAccelX; output.returnAccelY = sensOut.returnAccelY; output.returnAccelZ = sensOut.returnAccelZ; output.returnDistance = sensOut.returnDistance; return output; }
virtual void ClearSensorData(const std::string & sensorType, const std::string & sensorName){ Poco::Mutex::ScopedLock l(mtx); std::string key = SensorDataHelpers::sensorID(sensorType, sensorName); storage.erase(key); current.erase(key); sensors.erase(key); }
// // Main Autonomous Mode function. This function is called once, therefore a while loop that checks IsAutonomous and IsEnabled is used // to maintain control until the end of autonomous mode // void Autonomous() { // Initialize Smart Dashboard SmartDashboard::init(); autonomous = new AutonomousMode(AUTO_TIMEONLY, drive->myRobot, sensors, catapult, STOP_DISTANCE_RANGE, STOP_DISTANCE_ENCOD, STOP_DRIVING_TIME, CAMERA_LED); //myRobot->SetSafetyEnabled(false); //AxisCamera &camera = AxisCamera::GetInstance(); //Relay* redddlight = new Relay(4); //Timer* lighttimer = new Timer(); //lighttimer->Start(); while(IsAutonomous() && IsEnabled()) { /* if (lighttimer->Get()<=0.25) { redddlight->Set(redddlight->kForward); } else if(lighttimer->Get()<0.5){ redddlight->Set(redddlight->kOff); } else { lighttimer->Reset(); } */ feeder->GetInputs(); sensors->GetInputs(); //ColorImage *image; //image = camera.GetImage(); //SmartDashboard::PutNumber("Range Finder Distance", sensors->GetInch(sensors->range->GetVoltage())); //SmartDashboard::PutNumber("Range Finder Average Distance", sensors->GetDistance()); autonomous->GetInputs(); autonomous->ExecStep(); autonomous->SetOutputs(); //delete ℑ //delete image; Wait(0.01); /* DigitalInput *limitswitch = new DigitalInput(1); limitswitch->Get(); */ } }
int main(int argc, char **argv) { signal(SIGINT, stop); ConfigFile config("walk.yml"); config.useCommandArgs(argc, argv); walk.loadConfig(config); config.help(); try { Robots robots; cout << "Starting " << endl; robots.loadYaml("config.yml"); Robot *robot = robots["local"]; walk.setRobot(robot); Motors *motors = robot->getMotors(); Sensors *sensors = robot->getSensors(); cout << "Starting motors " << endl; motors->start(100); sensors->start(100); ms_sleep(100); while (true) { walk.tick(0.01); ms_sleep(10); } ms_sleep(25); cout << "Bye bye" << endl; robots.stop(); } catch(string exc) { cout << "Received exception " << exc << endl; exit(0); } }
Input_Out Run(Input_In input) { // Declaring all necessary structs Input_Out output; X360Controller_In xbIn; X360Controller_Out xbOut; Sensors_In sensIn; Sensors_Out sensOut; sensIn.resetGyro = xbOut.returnResetGyro; // Running to obtain necessary information sensOut = sensors->Run(sensIn); xbOut = controller->Run(xbIn); // Controller info output.returnX = xbOut.returnX; output.returnY = xbOut.returnY; output.returnRotation = xbOut.returnRotation; output.returnTurboMode = xbOut.returnTurboMode; output.returnLiftAmount = xbOut.returnLiftAmount; output.returnLiftTurbo = xbOut.returnLiftTurbo; output.returnLiftActive = xbOut.returnLiftActive; output.returnLiftManualControl = xbOut.returnLiftManualControl; output.returnLiftState = xbOut.returnLiftState; //Sensor info output.returnGyroAngle = sensOut.returnGyroAngle; output.returnAccelX = sensOut.returnAccelX; output.returnAccelY = sensOut.returnAccelY; output.returnAccelZ = sensOut.returnAccelZ; output.returnDistance = sensOut.returnDistance; output.returnUpperLiftSwitch = sensOut.returnUpperLiftSwitch; output.returnLowerLiftSwitch = sensOut.returnLowerLiftSwitch; return output; }
const SensorInfo* get_by_sensor(int sensor_id) { for(size_t i = 0; i < _v.size(); i++) if(_v[i].sensor_id == sensor_id) return &_v[i]; return NULL; }
void add(const SensorInfo& s) { _v.push_back(s); std::sort(_v.begin(), _v.end()); }
Sensors::iterator end() { return _v.end(); }
Sensors::iterator begin() { return _v.begin(); }
Head2ECoGMat::Head2ECoGMat(const Geometry& geo, const Sensors& electrodes, const std::string& id) { assemble_Head2ECoG(*this, geo, electrodes.getPositions(), geo.interface(id)); }
int main() { //cout << "Hello World" << endl; // prints Sensors *sense = new Sensors; Control *controller = new Control(); puts("In main"); /* prints Broked */ // sense->startSensorsThreads(); // // Motor *motor1 = new Motor(); // Motor *motor2 = new Motor(); // Motor *motor3 = new Motor(); // Motor *motor4 = new Motor(); // // motor1->init(1); // motor2->init(2); // motor3->init(3); // motor4->init(4); // sleep(5); // int i; // for (i = 0; i < 30; i++) { // printf("iteration: %d\n", i); // motor1->setPower(i); // //motor2->setPower(i); // //motor3->setPower(i); // //motor4->setPower(i); // usleep(50000); // } // motor1->setPower(0); // for (i = 0; i < 30; i++) { // printf("iteration: %d\n", i); // motor2->setPower(i); // usleep(50000); // } // motor2->setPower(0); // for (i = 0; i < 30; i++) { // printf("iteration: %d\n", i); // motor3->setPower(i); // usleep(50000); // } // motor3->setPower(0); // for (i = 0; i < 30; i++) { // printf("iteration: %d\n", i); // motor4->setPower(i); // usleep(50000); // } // motor4->setPower(0); // motor1->setPower(0); // motor2->setPower(0); // motor3->setPower(0); // motor4->setPower(0); // sense->stopThread(); // Remotehandler *remote = new Remotehandler(); sense->startSensorsThreads(); usleep(500000); controller->controlRun(sense); // while (1) { //// printf("1: %f 2: %f 3: %f 4: %f\n", remote->getch1(), //// remote->getch2(), remote->getch3(), remote->getch4()); // printf("angle x: %f y: %f z: %f\nanglespeed x: %f y: %f z: %f\n", // sense->getXAngle(), sense->getYAngle(), sense->getZAngle(), // sense->getXAngleSpeed(), sense->getYAngleSpeed(), // sense->getZAngleSpeed()); // usleep(500000); // } sleep(40); return 0; }
Head2ECoGMat::Head2ECoGMat(const Geometry& geo, const Sensors& electrodes, const Interface& i) { assemble_Head2ECoG(*this, geo, electrodes.getPositions(), i); }
Head2EEGMat::Head2EEGMat(const Geometry& geo, const Sensors& electrodes) { assemble_Head2EEG(*this, geo, electrodes.getPositions()); }