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