void
ServoGroup::setStarted(bool started)
{
IceUtil::Mutex::Lock lock(this->hw_mutex);
if(this->is_started == false && started == true)
{// start
static const size_t servo_gpio = 11;
this->mem_fd = pwm_open_devmem();
if(this->mem_fd == -1)
{
Ice::LoggerPtr logger =
Ice::Service::instance()->communicator()->getLogger();
string msg = "Unable to open /dev/mem, are you root?";
logger->error(msg);
throw runtime_error(msg);
}
// Enable ICLK clock
pwm_iclken_clock(this->mem_fd, true, true);
// Enable FCLK clock
pwm_fclken_clock(this->mem_fd, true, true);
// Set instances 10 and 11 to use the 13 Mhz clock
pwm_config_clock(this->mem_fd, true, true);
this->gpio_timer = pwm_mmap_instance(mem_fd, servo_gpio);
// Get the resolution for 20ms period (50Hz) PWM
this->resolution = pwm_calc_resolution(50, PWM_FREQUENCY_13MHZ);
pwm_config_timer(this->gpio_timer,
this->resolution,
this->current_duty); // middle position
}
else if(this->is_started == true && started == false)
{// stop
pwm_munmap_instance(this->gpio_timer);
// Disable ICLK clock
pwm_iclken_clock(this->mem_fd, false, false);
// Disable FCLK clock
pwm_fclken_clock(this->mem_fd, false, false);
pwm_close_devmem(this->mem_fd);
this->mem_fd = -1;
}
this->is_started = started;
}
int
IceApp::run(int argc, char *argv[])
{
RouterHelperPtr rh;
Glacier2::RouterPrx defaultRouter;
Ice::PropertiesPtr props = this->communicator()->getProperties();
Ice::LoggerPtr log = this->communicator()->getLogger();
// Try to create router with firewall support using Glacier service
try
{
rh = new RouterHelper(this->communicator());
defaultRouter = rh->getRouter();
}
catch(const Ice::Exception& ex)
{
ostringstream os;
ex.ice_print(os);
log->warning(os.str());
}
// Create object adapter for sensor callback and keyboard sensor servants
Ice::ObjectAdapterPtr adapter;
if(defaultRouter)
adapter = communicator()->createObjectAdapterWithRouter("Cockpit",
defaultRouter);
else
adapter = communicator()->createObjectAdapter("Cockpit");
if(this->visuals.init(log, props))
{
log->error("Visuals initialization failed");
return EXIT_FAILURE;
}
Ice::ObjectPrx objprx;
// Initialize keyboard sensor (and admin) servants
KeyboardAdminIPtr keyboard_admin = new KeyboardAdminI();
objprx = adapter->add(keyboard_admin,
this->communicator()->stringToIdentity("keyboard-sensor-admin"));
admin::StatePrx keyboard_admin_prx =
admin::StatePrx::uncheckedCast(objprx);
this->keyboard_sensor = new KeyboardSensorI(keyboard_admin_prx);
keyboard_admin->setSensorGroup(this->keyboard_sensor);
objprx =
adapter->add(this->keyboard_sensor,
this->communicator()->stringToIdentity("keyboard-sensor"));
adapter->activate();
admin::StatePrx state_prx;
// Initialize actuators list
// Our chassis has two motors
actuatorinfo_t cactinfo = this->connectToActuator("Chassis");
actuators::ActuatorGroupPrx cactuator_prx = cactinfo.first;
//actuators::ActuatorDescriptionSeq cactdescr = actinfo.second;
// Servo motor to rotate the head with cameras and front sonar
actuatorinfo_t hactinfo = this->connectToActuator("Head");
actuators::ActuatorGroupPrx hactuator_prx = hactinfo.first;
//actuators::ActuatorDescriptionSeq hactdescr = actinfo.second;
// Initialize joystick callback servant
sensorinfo_t info = this->connectToSensor("Joystick");
sensors::SensorGroupPrx sensor_prx = info.first;
sensors::SensorDescriptionSeq descr = info.second;
ActuatorControllerPtr joy_chassis_ctl;
ActuatorControllerPtr joy_head_ctl;
if(sensor_prx && !descr.empty())
{
actctlkey_t key = std::make_pair(descr[0].id, descr[0].type);
if(cactuator_prx)
{
joy_chassis_ctl.reset(new JoystickChassisCtl(cactuator_prx));
this->actuator_map.insert(std::make_pair(key,
joy_chassis_ctl.get()));
this->chassis_state_prx = cactuator_prx->getStateInterface();
this->chassis_state_prx->start();
}
if(hactuator_prx)
{
joy_head_ctl.reset(new JoystickHeadCtl(hactuator_prx));
this->actuator_map.insert(std::make_pair(key,
joy_head_ctl.get()));
state_prx = hactuator_prx->getStateInterface();
state_prx->start();
}
SensorFrameReceiverIPtr joystick_callback =
new SensorFrameReceiverI(descr[0].type,
descr[0].minvalue,
descr[0].maxvalue,
&this->buffer_queue,
&this->visuals.tile_manager,
&this->visuals.sensor_animations_map,
&this->actuator_map);
objprx =
adapter->add(joystick_callback, rh->makeId("joystickcallback"));
sensors::SensorFrameReceiverPrx joystick_callback_prx =
sensors::SensorFrameReceiverPrx::uncheckedCast(objprx);
sensor_prx->setSensorReceiver(joystick_callback_prx);
state_prx = sensor_prx->getStateInterface();
state_prx->start();
}
// Initialize keyboard callback servant
info = this->connectToSensor("Keyboard");
sensor_prx = info.first;
descr = info.second;
ActuatorControllerPtr kbd_chassis_ctl;
ActuatorControllerPtr kbd_head_ctl;
if(sensor_prx && !descr.empty())
{
actctlkey_t key = std::make_pair(descr[0].id, descr[0].type);
if(cactuator_prx)
{
kbd_chassis_ctl.reset(new KeyboardChassisCtl(cactuator_prx));
this->actuator_map.insert(std::make_pair(key,
kbd_chassis_ctl.get()));
state_prx = cactuator_prx->getStateInterface();
state_prx->start();
}
if(hactuator_prx)
{
kbd_head_ctl.reset(new KeyboardHeadCtl(hactuator_prx));
this->actuator_map.insert(std::make_pair(key,
kbd_head_ctl.get()));
state_prx = hactuator_prx->getStateInterface();
state_prx->start();
}
SensorFrameReceiverIPtr kbd_callback =
new SensorFrameReceiverI(descr[0].type,
descr[0].minvalue,
descr[0].maxvalue,
&this->buffer_queue,
&this->visuals.tile_manager,
&this->visuals.sensor_animations_map,
&this->actuator_map);
objprx =
adapter->add(kbd_callback,
this->communicator()->stringToIdentity("kbdcallback"));
sensors::SensorFrameReceiverPrx kbd_callback_prx =
sensors::SensorFrameReceiverPrx::uncheckedCast(objprx);
sensor_prx->setSensorReceiver(kbd_callback_prx);
state_prx = sensor_prx->getStateInterface();
state_prx->start();
}
// Initialize sonars callback servant
info = this->connectToSensor("Sonars");
sensor_prx = info.first;
descr = info.second;
if(sensor_prx && !descr.empty())
{
SensorFrameReceiverIPtr sonars_callback =
new SensorFrameReceiverI(descr[0].type,
descr[0].minvalue,
descr[0].maxvalue,
&this->buffer_queue,
&this->visuals.tile_manager,
&this->visuals.sensor_animations_map,
NULL);
objprx =
adapter->add(sonars_callback, rh->makeId("sonarscallback"));
sensors::SensorFrameReceiverPrx sonars_callback_prx =
sensors::SensorFrameReceiverPrx::uncheckedCast(objprx);
sensor_prx->setSensorReceiver(sonars_callback_prx);
admin::StatePrx state_prx = sensor_prx->getStateInterface();
state_prx->start();
}
// Initialize compass callback servant
info = this->connectToSensor("Compass");
sensor_prx = info.first;
descr = info.second;
if(sensor_prx && !descr.empty())
{
SensorFrameReceiverIPtr compass_callback =
new SensorFrameReceiverI(descr[0].type,
descr[0].minvalue,
descr[0].maxvalue,
&this->buffer_queue,
&this->visuals.tile_manager,
&this->visuals.sensor_animations_map,
NULL);
objprx =
adapter->add(compass_callback, rh->makeId("compasscallback"));
sensors::SensorFrameReceiverPrx compass_callback_prx =
sensors::SensorFrameReceiverPrx::uncheckedCast(objprx);
sensor_prx->setSensorReceiver(compass_callback_prx);
admin::StatePrx state_prx = sensor_prx->getStateInterface();
state_prx->start();
}
// Initialize camera callback servant
info = this->connectToSensor("Camera");
sensor_prx = info.first;
descr = info.second;
if(sensor_prx && !descr.empty())
{
const std::string prop_name = "Decoding.Pipeline";
std::string dec_pipeline = props->getProperty(prop_name);
if(dec_pipeline.empty())
{
log->warning(prop_name + " property not set. No video.");
}
else
{
this->video_decoder.initAndStart(argc, argv,
dec_pipeline.c_str(),
this->visuals.video_painters,
&this->buffer_queue);
SensorFrameReceiverIPtr camera_callback =
new SensorFrameReceiverI(descr[0].type,
descr[0].minvalue,
descr[0].maxvalue,
&this->buffer_queue,
&this->visuals.tile_manager,
&this->visuals.sensor_animations_map,
NULL);
objprx =
adapter->add(camera_callback, rh->makeId("cameracallback"));
sensors::SensorFrameReceiverPrx camera_callback_prx =
sensors::SensorFrameReceiverPrx::uncheckedCast(objprx);
sensor_prx->setSensorReceiver(camera_callback_prx);
state_prx = sensor_prx->getStateInterface();
state_prx->start();
}
}
this->mainloop();
// Be nice and stop chassis motors on exit
if(this->chassis_state_prx)
{
this->chassis_state_prx->stop();
this->chassis_state_prx = 0;
}
return EXIT_SUCCESS;
}
