int main(){
int a = 0;
Energy *energy0 = calloc(sizeof(Energy), 1);
Energy *energy1 = calloc(sizeof(Energy), 1);
//Energy *energy2 = calloc(sizeof(Energy), 1);
initEnergy(energy0, 1);
initEnergy(energy1, 1);
//initEnergy(energy2, 1);
setSensor(energy0, "ACa", "A", "", "/dev/ttyACM0");
setSensor(energy1, "ACb", "B", "", "/dev/ttyACM0");
//setSensor(energy5, "AC ", "C", "", "/dev/ttyACM0");
setTimer(energy0, 10);
setTimer(energy1, 10);
//setTimer(energy2, 10);
startEnergy(energy0);
startEnergy(energy1);
//startEnergy(energy2);
while(a < 60){
//system("clear");
printEnergy(energy0);
printEnergy(energy1);
//printEnergy(energy2);
a ++;
sleep(10);
}
//system("clear");
printf("stop \n");
stopEnergy(energy0);
stopEnergy(energy1);
//stopEnergy(energy2);
sleep(6);
printEnergy(energy0);
printEnergy(energy1);
//printEnergy(energy2);
}
//------------------------------------------------------------------------------
// deleteData() -- delete member data
//------------------------------------------------------------------------------
void EmissionPduHandler::deleteData()
{
if (sensor != nullptr) { sensor->event(oe::base::Component::SHUTDOWN_EVENT); }
setSensor(nullptr);
if (sensorModel != nullptr) { sensorModel->event(oe::base::Component::SHUTDOWN_EVENT); }
setSensorModel(nullptr);
if (antennaModel != nullptr) { antennaModel->event(oe::base::Component::SHUTDOWN_EVENT); }
setAntennaModel(nullptr);
}
void
RangeSensorWidget::selectSensor()
{
bool ok = false;
QString tmp = QInputDialog::getText(tr( "Select sensor at " ) + robotName_,
tr( "Sensor name" ),
QLineEdit::Normal, sensorName_, &ok, this );
if ( ok && !tmp.isEmpty() ) {
setSensor(tmp);
}
}
void QChain::createChain() {
std::vector<Vector2d> tmp(m_vertices.begin(), m_vertices.end());
tmp = Geometry::simplifyPolygon(tmp, 0.2);
m_vertices = std::vector<QPointF>(tmp.begin(), tmp.end());
std::vector<QPointF> pts = vertices();
size_t it = pts.size() - 1, prev = it - 1, next = 1;
for (size_t i = 0; i < pts.size(); i++) {
QPointF vec = pts[i] - pts[it];
QPointF vecp = pts[it] - pts[prev], vecn = pts[next] - pts[i];
auto fixture = std::make_unique<Box2DEdge>(this);
fixture->setVisible(false);
fixture->setShadowCaster(false);
b2EdgeShape &edge = fixture->edgeShape();
edge.Set(tob2Vec2(pts[it]), tob2Vec2(pts[i]));
if (QPointF::dotProduct(vec, vecp) > 0) {
edge.m_hasVertex0 = true;
edge.m_vertex0 = tob2Vec2(pts[prev]);
}
if (QPointF::dotProduct(vec, vecn) > 0) {
edge.m_hasVertex3 = true;
edge.m_vertex3 = tob2Vec2(pts[next]);
}
addFixture(std::move(fixture));
prev = it, it = i;
next = next + 1 < pts.size() ? next + 1 : 0;
}
qreal minx = INF, miny = INF, maxx = -INF, maxy = -INF;
for (QPointF p : vertices()) {
minx = std::min(minx, p.x());
miny = std::min(miny, p.y());
maxx = std::max(maxx, p.x());
maxy = std::max(maxy, p.y());
}
auto box = std::make_unique<Box2DBox>();
box->setSize(QSizeF(maxx - minx, maxy - miny));
box->setPosition(QPointF(minx, miny));
box->setVisible(false);
box->setSensor(true);
addFixture(std::move(box));
m_texture.setVertices(vertices());
}
void StaticLight::initialize(QWorld *w) {
auto fixture = std::make_unique<Box2DBox>();
fixture->setPosition(QPointF(-radius(), -radius()));
fixture->setSize(QSizeF(2 * radius(), 2 * radius()));
fixture->setSensor(true);
fixture->setVisible(false);
fixture->setShadowCaster(false);
addFixture(std::move(fixture));
assert(lightSystem());
lightSystem()->addLight(this);
QBody::initialize(w);
}
void ColladaBodyLoaderImpl::setLink(DaeNode* extNode, Link* parentLink, Body& body)
{
// joint and link and rigid are associated with attributes of name.
DaeNode* value = parser->findNode(static_cast<DaeLink*>(extNode)->name);
// Using the joint name only.
SgInvariantGroup* group = (!(parentLink->shape()) ? new SgInvariantGroup : static_cast<SgInvariantGroup*>(parentLink->shape()));
parser->createNode(value, static_cast<SgGroup*>(group));
if (!(parentLink->shape())) parentLink->setShape(group);
#ifdef _OLD_VERSION
setColdetModel(parentLink, group);
#endif
setSensor(extNode, parentLink, body);
}
void Bullet::initialize(QWorld* w) {
setBodyType(QBody::BodyType::Dynamic);
auto f = std::make_unique<Box2DBox>();
QSizeF size(10, 10);
f->setSize(size);
f->setPosition(-QPointF(0.5 * size.width(), 0.5 * size.height()));
f->setTextureSource(":/resources/punch.png");
f->setSensor(true);
f->setCollidesWith(
QFixture::CategoryFlag(QFixture::All & ~QFixture::Category2));
addFixture(std::move(f));
setRotation(180.0 / M_PI * atan2(-m_direction.x(), m_direction.y()) + 90.0);
QBody::initialize(w);
enqueueFunction(std::bind(&Bullet::onStepped, this));
}
//------------------------------------------------------------------------------
// copyData() -- copy member data
//------------------------------------------------------------------------------
void EmissionPduHandler::copyData(const EmissionPduHandler& org, const bool cc)
{
BaseClass::copyData(org);
if (cc) initData();
// Copy handler info
setEmitterName( org.emitterName );
setEmitterFunction( org.emitterFunction );
noTemplatesFound = org.noTemplatesFound;
defaultIn = org.defaultIn;
defaultOut = org.defaultOut;
// But clear out the rest (so we can start over as a new handler)
emPduExecTime = 0;
setSensor(nullptr);
setSensorModel(nullptr);
if (org.getSensorModel() != nullptr) {
simulation::RfSensor* tmp = org.getSensorModel()->clone();
setSensorModel(tmp);
tmp->unref();
}
setAntennaModel(nullptr);
if (org.getAntennaModel() != nullptr) {
simulation::Antenna* tmp = org.getAntennaModel()->clone();
setAntennaModel(tmp);
tmp->unref();
}
// ---
// copy the saved (N-1) data
// ---
setSavedEmissionSystemData( *( org.getSavedEmissionSystemData() ) );
for (unsigned int j = 0; j < MAX_EM_BEAMS; j++) {
setSavedEmitterBeamData( j, *( org.getSavedEmitterBeamData(j) ) );
for (int k = 0; k < MAX_TARGETS_IN_TJ_FIELD; k++) {
setSavedTrackJamTargetData(j, k, *( org.getSavedTrackJamTargetData(j,k) ) );
}
}
}
void
RangeSensorWidget::setRobot(const QString& _robot)
{
QString error;
bool ok = true;
// build the lookup string for the naming service
CosNaming::Name name;
name.length(1);
name[0].id = CORBA::string_dup(_robot.latin1());
// try binding the robots naming context
try {
CosNaming::NamingContext_var namingContext =
client_.resolveName<CosNaming::NamingContext>(name);
}
catch(const CORBA::Exception& e) {
std::ostringstream sstr;
sstr << "Robot " << _robot << endl
<< "Communication Failed." << endl
<< "CORBA exception: " << e << flush;
error = sstr.str().c_str();
ok = false;
}
if (ok) {
if (timer_) {
killTimer(timer_);
timer_ = 0;
}
menuFile_->setItemEnabled(groupIndex_, false);
robotName_ = _robot;
sensor_ = Miro::RangeSensor::_nil();
scan_ = NULL;
scanDescription_ = NULL;
setSensor(sensorName_);
}
else {
calcCaption();
QMessageBox::warning(this, "Couln't set robot:", error);
}
}
void MainWindow::onDataReceived(SensorData sensorData)
{
setSensor(sensorData);
}
//------------------------------------------------------------------------------
// updateIncoming() -- Process an emission system from incoming PDUs
//------------------------------------------------------------------------------
bool EmissionPduHandler::updateIncoming(const ElectromagneticEmissionPDU* const pdu, const EmissionSystem* const es, Nib* const nib)
{
simulation::Player* player = nib->getPlayer();
if (player == nullptr || noTemplatesFound) return false;
// ---
// Default sensor: process only one beam)
// ---
if (es->numberOfBeams > 0) {
const EmitterBeamData* bd = es->getEmitterBeamData(0);
// ---
// Use our template models to create the RfSensor and Antenna
// ---
if (getSensor() == nullptr && !noTemplatesFound) {
simulation::RfSensor* rp = getSensorModel();
simulation::Antenna* ap = getAntennaModel();
// If we have both the RF system and antenna models ...
if (rp != nullptr && ap != nullptr) {
rp->setAntenna(ap); // The Radar owns the antenna
// Reset
rp->reset();
ap->reset();
// Give the antenna list to the IPlayer
{
// First get the (top level) container gimbal
simulation::Gimbal* gimbal = player->getGimbal();
if (gimbal == nullptr) {
// Create the container gimbal!
gimbal = new simulation::Gimbal();
base::Pair* pair = new base::Pair("gimbal", gimbal);
gimbal->unref(); // pair owns it
player->addComponent(pair);
pair->unref(); // player owns it
}
// Add this antenna to the container gimbal
ap->container(gimbal);
base::Pair* pair = new base::Pair("antenna", ap);
ap->unref(); // pair owns it
gimbal->addComponent(pair);
pair->unref(); // top level gimbal owns it
}
// Give the sensor list to the IPlayer
{
// First get the (top level) sensor manager
simulation::RfSensor* sm = player->getSensor();
if (sm == nullptr) {
// Create the sensor manager
sm = new simulation::SensorMgr();
base::Pair* pair = new base::Pair("sensorMgr", sm);
sm->unref(); // pair owns it
player->addComponent(pair);
pair->unref(); // player owns it
}
// Add this system to the sensor manager
rp->container(sm);
base::Pair* pair = new base::Pair("sensor", rp);
rp->unref(); // pair owns it
sm->addComponent(pair);
pair->unref(); // sensor manager owns it
}
setSensor(rp);
}
// If we do NOT have both the RF system and antenna models ...
else {
noTemplatesFound = true;
std::cerr << "EmissionPduHandler::updateIncoming(): warning -- no template ";
if (rp == nullptr) std::cerr << "R/F model";
if (rp == nullptr && ap == nullptr) std::cerr << " or ";
if (ap == nullptr) std::cerr << "antenna model";
std::cerr << " found for emitterName: " << es->emitterSystem.emitterName;
std::cerr << std::endl;
}
}
// ---
// Update the IPlayer's sensor/antenna structures with the PDU data
// ---
simulation::RfSensor* rfSys = getSensor();
if (rfSys != nullptr && !noTemplatesFound) {
simulation::Antenna* antenna = rfSys->getAntenna();
// reset the timeout clock for this Iplayer's emissions
setEmPduExecTime(player->getSimulation()->getExecTimeSec());
rfSys->setFrequency( bd->parameterData.frequency );
rfSys->setBandwidth( bd->parameterData.frequencyRange );
// DPG ### Setting peak power to the effected radiated power from the PDU,
// so our transmitter loss and antenna gain should both be set to 0 dB (real 1.0).
base::Decibel db( bd->parameterData.effectiveRadiatedPower ); // dBm (dB milliwatts)
rfSys->setPeakPower( db.getReal() / 1000.0f );
rfSys->setPRF( bd->parameterData.pulseRepetitiveFrequency );
rfSys->setPulseWidth( bd->parameterData.pulseWidth / 1000000.0f );
if ( bd->beamData.beamAzimuthCenter == 0 &&
(bd->beamData.beamAzimuthSweep == 0 || bd->beamData.beamAzimuthSweep >= base::PI)
) {
// circular scan
antenna->setRefAzimuth( 0 );
antenna->setRefElevation( bd->beamData.beamElevationCenter );
antenna->setScanMode( simulation::ScanGimbal::CIRCULAR_SCAN );
antenna->setCmdRate( (24.0f * static_cast<double>(base::Angle::D2RCC)), 0 ); // default rates
}
else {
// Standard search volume parameters
antenna->setRefAzimuth( bd->beamData.beamAzimuthCenter );
antenna->setRefElevation( bd->beamData.beamElevationCenter );
// note that beamElevationSweep corresponds to scanHeight; setSearchVolume is expecting el component to be scanHeight+.5*barspacing
antenna->setSearchVolume( bd->beamData.beamAzimuthSweep * 2.0f, bd->beamData.beamElevationSweep * 2.0f);
}
// IPlayer's transmit (when they're active) but don't need to receive
if (pdu->header.protocolVersion >= NetIO::VERSION_7) {
rfSys->setTransmitterEnableFlag((bd->beamStatus == BS_ACTIVE));
}
else {
rfSys->setTransmitterEnableFlag(true);
}
rfSys->setReceiverEnabledFlag(false);
}
}
// No beam data -- turn off the transmitter and receiver
else {
simulation::RfSensor* rfSys = getSensor();
if (rfSys != nullptr) {
rfSys->setTransmitterEnableFlag(false);
rfSys->setReceiverEnabledFlag(false);
}
}
return true;
}
