C++ control函数代码示例

bool socket_receive_fds(int fd, std::vector<int> *fds)
{
    char dummy;

    struct iovec iov;
    iov.iov_base = &dummy;
    iov.iov_len = 1;

    std::size_t n_fds = fds->size();
    std::vector<char> control(sizeof(struct cmsghdr) + sizeof(int) * n_fds);

    struct msghdr msg;
    msg.msg_name = nullptr;
    msg.msg_namelen = 0;
    msg.msg_iov = &iov;
    msg.msg_iovlen = 1;
    msg.msg_flags = 0;
    msg.msg_control = control.data();
    msg.msg_controllen = control.size();

    struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
    cmsg->cmsg_len = msg.msg_controllen;
    cmsg->cmsg_level = SOL_SOCKET;
    cmsg->cmsg_type = SCM_RIGHTS;

    if (recvmsg(fd, &msg, 0) < 0) {
        return false;
    }

    int *data = reinterpret_cast<int *>(CMSG_DATA(cmsg));
    n_fds = (cmsg->cmsg_len - sizeof(struct cmsghdr)) / sizeof(int);
    if (n_fds != fds->size()) {
        // Did not receive correct amount of file descriptors
        return false;
    }

    for (std::size_t i = 0; i < n_fds; ++i) {
        (*fds)[i] = data[i];
    }

    return true;
}

void
process(Tokenrow *trp)
{
	int anymacros = 0;

	for (;;) {
		if (trp->tp >= trp->lp) {
			trp->tp = trp->lp = trp->bp;
			outp = outbuf;
			anymacros |= gettokens(trp, 1);
			trp->tp = trp->bp;
		}
		if (trp->tp->type == END) {
			if (--incdepth>=0) {
				if (cursource->ifdepth)
					error(ERROR,
					 "Unterminated conditional in #include");
				unsetsource();
				cursource->line += cursource->lineinc;
				trp->tp = trp->lp;
				genline();
				continue;
			}
			if (ifdepth)
				error(ERROR, "Unterminated #if/#ifdef/#ifndef");
			break;
		}
		if (trp->tp->type==SHARP) {
			trp->tp += 1;
			control(trp);
		} else if (!skipping && anymacros)
			expandrow(trp, NULL);
		if (skipping)
			setempty(trp);
		puttokens(trp);
		anymacros = 0;
		cursource->line += cursource->lineinc;
		if (cursource->lineinc>1) {
			genline();
		}
	}
}

// Expand simple expressions like new int[3][5] and new Object[2][nonConLen].
// Also handle the degenerate 1-dimensional case of anewarray.
Node* Parse::expand_multianewarray(ciArrayKlass* array_klass, Node* *lengths, int ndimensions, int nargs) {
  Node* length = lengths[0];
  assert(length != NULL, "");
  Node* array = new_array(makecon(TypeKlassPtr::make(array_klass)), length, nargs);
  if (ndimensions > 1) {
    jint length_con = find_int_con(length, -1);
    guarantee(length_con >= 0, "non-constant multianewarray");
    ciArrayKlass* array_klass_1 = array_klass->as_obj_array_klass()->element_klass()->as_array_klass();
    const TypePtr* adr_type = TypeAryPtr::OOPS;
    const TypeOopPtr*    elemtype = _gvn.type(array)->is_aryptr()->elem()->make_oopptr();
    const intptr_t header   = arrayOopDesc::base_offset_in_bytes(T_OBJECT);
    for (jint i = 0; i < length_con; i++) {
      Node*    elem   = expand_multianewarray(array_klass_1, &lengths[1], ndimensions-1, nargs);
      intptr_t offset = header + ((intptr_t)i << LogBytesPerHeapOop);
      Node*    eaddr  = basic_plus_adr(array, offset);
      store_oop_to_array(control(), array, eaddr, adr_type, elem, elemtype, T_OBJECT);
    }
  }
  return array;
}

int Jump(int stgnum,Agent_status *agent, node_t *trees,int auto_or_mamual,Girl *Girl_status)
{
    //慣性を監視
    //    mvprintw(0,0,"intertia = %d",agent->INTERTIA);

    int highest = 5;

    for(;highest > 0;highest--)
    {
        if(mvinch(agent->Y-3,agent->X) != 'I')
        {
            agent->Y -= 1;
            agent->Jumpflag = 1;
        }
        moveRoL(agent);
        control(stgnum,agent,trees,auto_or_mamual,Girl_status);
        //            usleep(50000);
    }
    return 0;
}

void AgentCore::algorithmCallback(const ros::TimerEvent &timer_event) {
  consensus();  // also clears the received statistics container
  control();  // also publishes virtual agent pose and path
  guidance();
  dynamics();  // also publishes agent pose and path

  waitForSlotTDMA(slot_tdma_*agent_id_);  // sync to the next transmission TDMA slot (agent dependent)

  // publishes the last estimated statistics in the proper TDMA slot
  formation_control::FormationStatisticsStamped msg;
  msg.header.frame_id = agent_virtual_frame_;
  msg.header.stamp = ros::Time::now();
  msg.agent_id = agent_id_;
  msg.stats = estimated_statistics_;
  stats_publisher_.publish(msg);

  std::stringstream s;
  s << "Estimated statistics published.";
  console(__func__, s, DEBUG);
}

void casede(void)
{
	int i, req;
	Offset savoff;

	req = '.';
	lgf++;
	skip();
	if ((i = getrq()) == 0)
		goto de1;
	if ((offset = finds(i)) == 0)
		goto de1;
	if (newmn)
		savslot = newmn;
	else
		savslot = findmn(i);
	savname = i;
	if (ds)
		copys();
	else
		req = copyb();
	clrmn(oldmn);
	if (newmn) {
		if (contabp[newmn].rq)
			munhash(&contabp[newmn]);
		contabp[newmn].rq = i;
		maddhash(&contabp[newmn]);

	}
	if (apptr) {
		savoff = offset;
		offset = apptr;
		wbf((Tchar) IMP);
		offset = savoff;
	}
	offset = dip->op;
	if (req != '.')
		control(req, 1);
de1:
	ds = app = 0;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, ROS_PACKAGE_NAME);
  ros::NodeHandle node;

  ros::Publisher cmd_pub = node.advertise<ackermann_msgs::AckermannDriveStamped>(
      "ackermann_cmd",
      1000
    );
  ros::Publisher cte_pub = node.advertise<std_msgs::Float64>(
      "cte",
      1000
    );
  aav_control::QuinticControl control(&cmd_pub, &cte_pub);
  /*
  ros::Subscriber sub = node.subscribe(
      "odometry/filtered",
      1000,
      &aav_control::QuinticControl::updateOdometry,
      &control
    );
  */
  aav_control::GazeboStateForwarder forwarder(control, "car");
  ros::Subscriber sub = node.subscribe(
      "/gazebo/model_states",
      1000,
      &aav_control::GazeboStateForwarder::forwardState,
      &forwarder
    );

  actionlib::SimpleActionServer<aav_msgs::DoQuinticPathAction> server(
      node,
      "control",
      boost::bind(&aav_control::QuinticControl::updateGoal, &control, _1),
      false
    );
  server.start();

  ros::spin();
  return 0;
}

int main (void)
{
  int fd = initMW();

  ThinkGearStreamParser ctx;
  initParser(&ctx);

#ifdef OUTPUT_UDP
  initUDP();
#endif

  int res, i, n;
  unsigned char bytes[BUFSIZE];
  while (1) {
    n = read (fd, bytes, BUFSIZE); // read up to BUFSIZE bytes from the device

    if (n > 0) {
#ifdef __DEBUG
      fprintf (stderr, "%i bytes read\n", n);
#endif
      for (i = 0; i < n; i++) {
        res = THINKGEAR_parseByte(&ctx, bytes[i]);
        if (res < 0) {
          fprintf (stderr, "error parsing byte: %i\n", res);
          initParser(&ctx);
          continue;
        }
        control();  // output robot control char
      }
    } else
    if (n < 0) {
      fprintf (stderr, "error %d reading %s: %s\n", errno, MINDWAVEPORT,
        strerror (errno));
      return (1);
    }

    usleep ((n * 100) + 1000);  // sleep approx 100 uS per char transmit + 1kuS
  }

  return (0);
}

void *elfloader(char *path)
{
	
	/*Load the program into memory first*/

	int fd = open(RFILESYS,path, "or");
	if (fd == SYSERR)
	{
		kprintf("Could not open the file\n");
		return (void *)SYSERR;
	}
	
	int32 filesize = control(RFILESYS, RFS_CTL_SIZE, fd, 0);

	char *filestart;
	
	filestart = getmem(filesize);

	if(filestart == (char *)SYSERR)
	{
		kprintf("Not enough memory to load the entire file\n");
		return (void *)SYSERR;
	}


	int rc = read(fd, filestart, filesize);

	if(rc == filesize)
	{
		kprintf("successfully read the file into memory at %u\n");
	}
	else
	{
		kprintf("Error reading file into memory\n");
		close(fd);
		return (void *)SYSERR;
	}

	close(fd);
return (void *)filestart;
}

/* MotionController::advance: advance motion controller by the given frame, return true when reached end */
bool MotionController::advance(double deltaFrame)
{
   if (m_boneCtrlList == NULL && m_faceCtrlList == NULL)
      return false;

   /* apply motion at current frame to bones and faces */
   control((float) m_currentFrame);

   /* advance the current frame count */
   /* store the last frame to m_previousFrame */
   m_previousFrame = m_currentFrame;
   m_currentFrame += deltaFrame;
   if (m_currentFrame >= m_maxFrame) {
      /* we have reached the last key frame of this motion */
      /* clamp the frame to the maximum */
      m_currentFrame = m_maxFrame;
      /* return finished status */
      return true;
   }
   return false;
}

void startup(void)
{
	printf("System is Starting...\n");

	printf("Initializing Serial Port...\n");
	if(roboLinkInit()>0)
	{
		printf("RoboLink is Ready.\n");
	}
	else
	{
		printf("RoboLink Init Failed.\n");
	}
	boost::thread udp(&udpServerTask);
	printf("UDP Server is Running!\n");
	boost::thread tcp(&tcpServerTask);
	printf("TCP Server is Running!\n");
	boost::thread control(&roboControlLoop);
	printf("Robot Control Loop is Running!!");
	triZero();
}

TEST_F(LearningUtilsTest, CC7BitKnob) {
    // Standard CC 7-bit knobs show up as a MIDI_CC message, single channel,
    // single control and a variety of values in the range of 0x00 to 0x7F.
    // Status: 0x81 Control: 0x01
    addMessage(MIDI_CC | 0x01, 0x10, 0x7F);
    addMessage(MIDI_CC | 0x01, 0x10, 0x70);
    addMessage(MIDI_CC | 0x01, 0x10, 0x60);
    addMessage(MIDI_CC | 0x01, 0x10, 0x50);
    addMessage(MIDI_CC | 0x01, 0x10, 0x60);
    addMessage(MIDI_CC | 0x01, 0x10, 0x50);
    addMessage(MIDI_CC | 0x01, 0x10, 0x40);

    ConfigKey control("[Test]", "SomeControl");
    MidiInputMappings mappings =
            LearningUtils::guessMidiInputMappings(control, m_messages);

    ASSERT_EQ(1, mappings.size());
    EXPECT_EQ(MidiInputMapping(MidiKey(MIDI_CC | 0x01, 0x10),
                               MidiOptions(), control),
              mappings.at(0));
}

int main(int argc, char *argv[])
{
	QApplication app(argc, argv);

	control();

	ControlDlg *controlDlg = new ControlDlg;
	StatusDlg *statusDlg = new StatusDlg(NULL, controlDlg);

	QTimer screenUpdate;
	QObject::connect(&screenUpdate, SIGNAL( timeout(void) ), controlDlg, SLOT ( updateScreen(void) ));
	QObject::connect(&screenUpdate, SIGNAL( timeout(void) ), statusDlg, SLOT ( updateScreen(void) ));

	screenUpdate.setSingleShot(false);
	screenUpdate.start(100);

	statusDlg->show();
	return app.exec();

	return 0;
}

void or(char *rs,char *rt,char *rd)
{
  int j = 0;
  char address_buffer[33];
  opcode = "000000";
  shamt = "00000";
  function = "100101";

  sscanf(rs, "%d", &j);
  to_Binary(j,5,"rs");

  sscanf(rt, "%d", &j);
  to_Binary(j,5,"rt");
  
  sscanf(rd, "%d", &j);
  to_Binary(j,5,"rd");

  snprintf( address_buffer, sizeof( address_buffer ), "%s%s%s%s%s%s", opcode, rs_out, rt_out, rd_out, shamt, function);
  strcpy(address, address_buffer);
  control();
}

void
CameraTrigger::start()
{
	// enable immediate if configured that way
	if (_mode == 2) {
		control(true);
	}

	// Prevent camera from sleeping, if triggering is enabled
	if (_mode > 0 && _mode < 4) {
		turnOnOff();
		keepAlive(true);

	} else {
		keepAlive(false);
	}

	// start to monitor at high rate for trigger enable command
	work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1));

}