예제 #1
0
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;
}
예제 #2
0
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;
}