Exemple #1
0
int main()
{
	Blink blink;
	Flash flash;
	Zigbee zigbee;
	clock_begin();
	// usart1.init(0x33, true);
	usart1.init(12, true);
//	usart1.setTrigger(';');
	// usart1.setTriggerTime(60);
	// _delay_ms(1000);
	blink.start(0);
	usart1.send("zigbee_ready(a);");
	_delay_ms(50);
	while ((addr_a = zigbee.addr()) == 0xfffe)
	{
		blink.onloop();
		_delay_ms(100);
	}
	flash.start(0);
	while (!got_addr)
	{
		flash.onloop();
//		usart1.send("zigbee_ready(a);");
		_delay_ms(1000);
		checkCmd(usart1);
	}
	// while (addr_b == 0xfffe)
	// {
	// 	flash.onloop();
	// 	checkCmd(usart1);
	// 	_delay_ms(500);
	// }
	// usart1.send("got(a);");
	// _delay_ms(500);

	// lcd.dis("Waiting...");

//		usart1.send("zigbee_ready(a);");

	DDRA = 0xff;
	PORTA = 0x02;
	while (1)
	{
		motor.onloop();
		checkCmd(usart1);
	}
}
Exemple #2
0
void Client::getMessage()
{
	QDataStream in(tcpSocket);
	in.setVersion(QDataStream::Qt_4_0);

	QString message;
	in >> message;

	enum class COMMAND { NONE, USERLIST};
	COMMAND cmd = COMMAND::NONE;

	QStringRef checkCmd(&message, 0, 5);
	if (checkCmd == "_LST_")
		cmd = COMMAND::USERLIST;

	QStringList commandList;

	switch (cmd)
	{
	case COMMAND::USERLIST:
		commandList = message.split(" ", QString::SkipEmptyParts);
		commandList.removeFirst();
		ui.userList->clear();
		for (auto i : commandList)
		{
			new QListWidgetItem(i, ui.userList);
		}
		break;
	default:
		//in >> message;

		new QListWidgetItem(message, ui.messageList);
		ui.messageList->scrollToBottom();
	}
}
Exemple #3
0
void GameHandler::handle(int64 uid, string &cmd) {
	if (cmd.substr(0,3)=="ev,") {
		nh_->eq_->pushStringEvent(cmd.substr(3));
	} else if (cmd=="ka") {
		// keep alive message, do nothing
	} else if (cmd.substr(0, 22) == "<policy-stat-request/>") {
		nh_->sendFdString(fd_, "<policy-stat-response/>", 23);
		nh_->closeConnection(fd_);
	} else {
		vector<string> tokens;
		static string delims = ",";
		tokenize(cmd, tokens, delims);
		if (tokens.size()<1) return;
		string &cmd = tokens[0];
		if (checkCmd(cmd, "a", tokens, 2)) {
			int nid = -1;
			if (safe_atoi(tokens[1].c_str(), nid) && uid==-1 && nh_->registerConnection(fd_, nid)) {  // register firstly
				nh_->sendIntSizedFdString(fd_, string("pass"));
				UpdateWorkingStatus::addEvent(nh_->eq_, UpdateWorkingStatus_GH_Conn, nid);
			} else {
				nh_->sendIntSizedFdString(fd_, string("fail"));
			}
		}
	}
}
Exemple #4
0
QSqlQuery EncryptedDb::exec(const QString &query)
{
    QSqlQuery retQSqlQuery = PlainDb::exec(query);
    if (checkCmd(query))
        appendToEncrypted(query);

    return retQSqlQuery;
}
void mainControlSM(ros::NodeHandle &n)
{
    stream::info.open("info.txt");
    //PUBLISHER FOR MOTIONCONTROL
    ros::Publisher robo1Com = n.advertise<robot_soccer::controldata>("robot1Com", 5);
    ros::Publisher robo2Com = n.advertise<robot_soccer::controldata>("robot2Com", 5);

    //SUBSCRIBER FROM VISION
    ros::Subscriber vision_subscriber = n.subscribe("vision_data", 5, visionCallback);
    (void*)vision_subscriber;

    ros::Rate loop_rate(TICKS_PER_SEC);
    ros::Subscriber gameCmdSub = n.subscribe("game_cmd", 5, gameCmdCallback);
    (void*)gameCmdSub;
//    Strategies strategies;
//    Skills skill(RobotType::ally2);
//    skill.aim();
//      Plays play2(RobotType::ally2);
//      play2.playGoalie();
      Plays play1(RobotType::ally1);
      play1.rushGoal();
    while (ros::ok()) {
//        checkGCFlags(strategies);

        if (visionUpdated) {
            visionUpdated = false;
            field.updateStatus(visionStatus_msg);
//            strategies.tick();
            play1.tick();
//            play2.tick();
//            skill.tick();
        }
        if (sendCmd_Rob1) {
            sendCmd_Rob1 = false;
            checkCmd(cmdRob1);
            robo1Com.publish(cmdRob1);
        }
        if (sendCmd_Rob2){
            sendCmd_Rob2 = false;
            checkCmd(cmdRob2);
            robo2Com.publish(cmdRob2);
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
}
void predictSM(ros::NodeHandle &n)
{
    //PUBLISHER FOR MOTIONCONTROL
    ros::Publisher robo1Com = n.advertise<robot_soccer::controldata>("robot1Com", 5);
    ros::Publisher robo2Com = n.advertise<robot_soccer::controldata>("robot2Com", 5);

    //SUBSCRIBER FROM FILTERED VISION
    ros::Subscriber vision_subscriber = n.subscribe("filteredvision_data", 5, filterCallback);
    (void*)vision_subscriber;

    ros::Rate loop_rate(TICKS_PER_SEC);
    int count = 0;

    bool dataInitialized = false;

    ros::Subscriber gameCmdSub = n.subscribe("game_cmd", 5, gameCmdCallback);
    (void*)gameCmdSub;
    Plays play(RobotType::ally2);
    play.playGoalie();
  while (ros::ok()) {
//       checkGCFlags(strategies);

      if (predictedUpdated) {
          predictedUpdated = false;
          dataInitialized = true;
          field.updateStatus(predicted);
      }
      play.tick();

      if (sendCmd_Rob1) {
          sendCmd_Rob1 = false;
          checkCmd(cmdRob1);
          robo1Com.publish(cmdRob1);
      }
      if (sendCmd_Rob2){
          sendCmd_Rob2 = false;
          checkCmd(cmdRob2);
          robo2Com.publish(cmdRob2);
      }
      ros::spinOnce();
      loop_rate.sleep();
  }
}
Exemple #7
0
/*
 * This function commints the first step of the program.
 * fp - the name of the file that contains the assembler code.
 */
void firstStep(FILE* fp)
{
	int i = 0, z = 1, cmdNum, repeatCmd = 0; /* number of command*/
	int* addType;
	bool errFlag = FALSE, stepOneEnd = FALSE;
	line ln[lineVar], lastOperand;/* last read line*/
	initiateLine(ln);
	IC = 0; DC = 0;
	while(stepOneEnd == FALSE && ++lineNumber){
		z = 1;
		getLine(fp, ln, &stepOneEnd);
		QUE_setPtrToHead();
		if(strcmp(ln[z].word, "EMPTY") == 0)
			continue;
		if((ln[z].word[0] == ';' && strcmp(ln[0].word, "NULL") == 0) || (ln[0].word[0] == ';' ))
			continue;
		if(isLabelOK(ln[z - 1])){
			if(ln[z].word[0] != '.')
				insertToSymbolTable(labelTABLE, ln[z - 1].word, IC, IC + DC);
			else
				insertToSymbolTable(labelTABLE, ln[z - 1].word, DC, IC + DC);
		}
		if(ln[z].word[0] == '.'){ /* in case of guide sentence*/
			guideSentence(ln);
			continue;
		}
		if((cmdNum = checkCmd(ln[z], &repeatCmd)) == -1) continue;
		if(!isArgsNumFits(cmd[cmdNum].args, ln)) errFlag = TRUE;
		addType = calloc(cmd[cmdNum].args, sizeof(int));
		for(i = 0; i < cmd[cmdNum].args; i++){
			addType[i] = checkAddresingType(ln[++z].word);
			if(!isAddressingTypeOK(cmdNum, addType[i], i)) errFlag = TRUE;
			if(i == 0 && !(strcmp(ln[z].word, "$$") == 0)){
				lastOperand = ln[z];
				lastOperand.wordIdx = addType[0];
			}
		}
		if(errFlag == FALSE){
			setLineInMemory(cmdNum, addType, ln[2],ln[3], lastOperand);
			if(repeatCmd == 2){
				setLineInMemory(cmdNum, addType, ln[2],ln[3], lastOperand);
				if(addType[0] == 1 || addType[0] == 1){
					if(mem[IC- cmd[cmdNum].args].fieldNum == 1)
						dupLineLabels(IC- cmd[cmdNum].args );
					else
						dupLineLabels(IC- cmd[cmdNum].args - 1);
				}
			}
		}else errFlag = TRUE;
		free(addType);
		freeLine(ln);
	}
	mem[IC].fieldNum = -1;
	guidel[DC].string = endOfData;
}
/**
 * @brief Main
 *
 *  Initializes Bluecontroller and checks the commands.
 */
void main(void)
{
	bt_init();

	/* Write data persistent EEPROM/Flash => only necessary one time */
	//bt_setut();
	
	while(1) {
		checkCmd();
	}
}
//receive and resolve the command
void Interface::processCmd(void)
{
  static unsigned char cmd[7]={
    0,0,0,0,0,0,0    };//'R'+type+shift+red+green+blue+ASCII/index
  unsigned char receiveOK = 0;

  //check the commmand
  receiveOK = checkCmd(cmd);

  //reslove the command
  if(receiveOK){
    resolveCmd(cmd);
  }
}
int parser::parseToken(string& str) {
    string cmd = checkCmd(str);
    if (!strcmp(cmd.c_str(), "SWEEP")) {
        #ifdef DEBUG
        cout << "SWEEP: " << endl;
        #endif
        parseSWEEPToken(str);
    }
    else if (!strcmp(cmd.c_str(), "OPTIMIZE")) {
        parseOPTIMIZEToken(str);
    }
    else {
        #ifdef DEBUG
        cout << "UserToken: " << str.c_str() << endl;
        #endif
        parseUserToken(str);
    }
}
Exemple #11
0
bool Client::handleServer()
{
  static char buff[32768];
  int len;

  std::memset(buff, '\0', 32768);

  if ((len = read(_socket, buff, 32767)) <= 0) {
  
    if (close(_socket) == -1)
      std::cerr << "Error : close failed" << std::endl;

    throw NException("Error : read failed");
  }

  std::string buffer(buff);

  if (!buffer.empty() && buffer[buffer.length() - 1] == '\n')
    buffer.erase(buffer.length() - 1);
  _ans = buffer;

  std::cout << "\033[1;31m[SERVER] : " << buffer << "\033[0m" << std::endl;

  if (_step == GAME_STEP) {
    if (buffer == "mort")
      {
	std::cout << "You died" << std::endl;
	return false;
      }

  } else {
    checkCmd(buff);
    if (_step == Client::ERROR_STEP)
      return false;
  }

  checkAns();
  
  return true;
}
void property_recv(void *inContext)
{
  uint16_t len = 0;
  char property[150];
  
  app_context_t * app_context = (app_context_t*)inContext;
  
  while(1)
  {  
    if(!app_context->appStatus.arrayentStatus.isCloudConnected){
      mico_thread_sleep(1);
      continue;
    }

    len = sizeof(property);
    memset(property, 0, len);
    if(ARRAYENT_SUCCESS == ArrayentRecvProperty(property,&len,10000)){//receive property data from server
      user_log("recv property=%s\n\r",property);
      if(0 != checkCmd(property, inContext)){//parse cmd success
        user_log("checkCmd error: property=%s", property);
      }
    }
  }
}
Exemple #13
0
unsigned int kfi_getPid(const char *proc, unsigned int ppid)
{
    bool error = false;
    unsigned int pid = 0;
    static int pid_c = -1, ppid_c = -1, time_c = -1, cmd_c = -1;

    char cmd[BUFSIZE + 1];
    FILE *p;

    /* If this function has been run before, and we know the column positions, then we can grep for just our command */
    if(-1 != pid_c && -1 != ppid_c && -1 != time_c && -1 != cmd_c)
        snprintf(cmd, BUFSIZE, "ps -eaf | grep %s", proc);
    else
        strcpy(cmd, "ps -eaf");

    if(NULL != (p = popen(cmd, "r")))
    {
        char line[BUFSIZE + 1];
        int c = 0;
        char *linep = NULL, *token = NULL;

        /* Read 1st line to determine columns... */
        if((-1 == pid_c || -1 == ppid_c || -1 == time_c || -1 == cmd_c) && NULL != fgets(line, BUFSIZE, p))
        {
            for(linep = line; - 1 == pid_c || -1 == ppid_c || -1 == time_c || -1 == cmd_c; linep = NULL)
                if(NULL != (token = strtok(linep, " \t\n")))
                {
                    if(0 == strcmp("PID", token))
                        pid_c = c;
                    else if(0 == strcmp("PPID", token))
                        ppid_c = c;
                    else if(NULL != strstr("TIME", token))
                        time_c = c;
                    else if(0 == strcmp("COMMAND", token) || 0 == strcmp("CMD", token))
                        cmd_c = c;
                    c++;
                }
                else
                    break;
        }

        /* If all column headings read, then look for details... */
        if(-1 != pid_c && -1 != ppid_c && -1 != time_c && -1 != cmd_c)
            while(NULL != fgets(line, BUFSIZE, p) && !error)
            {
                int found = 0, ps_pid = 0, offset = 0;

                c = 0;
                for(linep = line; FOUND_ALL != found; linep = NULL)
                    if(NULL != (token = strtok(linep, " \t\n")))
                    {
                        if(c == pid_c)
                        {
                            found |= FOUND_PID;
                            ps_pid = atoi(token);
                        }
                        else if(c == ppid_c)
                        {
                            if(((unsigned int)atoi(token)) != ppid)
                                break;
                            found |= FOUND_PPID;
                        }
                        else if(c == time_c)
                            offset = isdigit(token[0]) ? 0 : 1;
                        else if(c == (cmd_c + offset))
                        {
                            if(0 != checkCmd(proc, token))
                                break;
                            found |= FOUND_CMD;
                        }
                        c++;
                    }
                    else
                        break;

                if(FOUND_ALL == found)
                {
                    if(pid)
                        error = true;
                    else
                        pid = ps_pid;
                }
            }
        pclose(p);
    }

    return error ? 0 : pid;
}
Exemple #14
0
void serialUI(void) {


	if (commandRetrived == 1) {

		if (commandType == 0) {
			checkCmd();
			commandRetrived = 0;
		}

		if (commandType == 1) {
			if (setCmd == 0) {								//this is to send in data. the data is not yet flashed
				led_switch(2);								//data can be send in at any time also at flight time
															//it will be used with the next cycle
				print_uart0("FCm0;storing setting  %d;00#",setupCache.settingNum);
				setToInSettings();							//fill the runtime struct with the settings
				setTempToInSetting(setupCache.settingNum);	//save settings to ram cache
				commandRetrived = 0;
			}

			if (setCmd == 1) {
				print_uart0("FCm0;flashing settings;00#");	//here we write all the settings from ram to flash
				enginesOff(); 								//this is to save some of the flashing cycles.
				led_switch(3);								//we could flash with any retrieved setting but that
				engineStatus = 0;							//just helps abusing the flash cycles so its a manual
															//step to really save to flash
				writeSetup();
				ADCStandstillValues();						//also we reset the ADCOffset
				commandRetrived = 0;
				initFCRuntime();
				ledTest();
			}
		}

		if (commandType == 2) {
			updateAcdRate();
			commandRetrived = 0;
		}
		if (commandType == 3) {
			retriveSetting();
			printSettings();
			commandRetrived = 0;
		}

		if (commandType == 4) {
			if (I2CcmdType == I2CMODE_WRITEADDRESS){
				if (updateYGE == 0) {
					I2C0Mode = I2CMODE_WRITEADDRESS;
					I2C0State = 0;
					updateYGE = 1;
					I2C0Start();
				}
			}

			if (I2CcmdType == I2CMODE_STARTUP_TARGET){
				if (updateYGE == 0) {
					I2C0Stop();
					I2C0Mode = I2CMODE_WRITEADDRESS;
					I2C0State = 0;
					updateYGE = 1;
					I2C0Start();
				}
			}
			commandRetrived = 0;
		}

	}









}
bool QueryRoomHandler::handle(CmdTask& task)
{
	srand((int) time(0));
	unsigned int tmp_user[4] = {0};

		
	tmp_user[0] = (rand() % (1000-800+1))+ 800;
	tmp_user[1] = (rand() % ( 500-300+1))+ 300;
	tmp_user[2] = (rand() % ( 300-200+1))+ 200;
	tmp_user[3] = (rand() % ( 200-100+1))+ 100;

	DataXCmdPtr& pCmd = task.pCmd;

	if(!pCmd)
	{
		LOG4CPLUS_ERROR(FLogger, "convert command to dataxcmd failed.");

		return false;
	}

	int rst = checkCmd(pCmd, string("GetDirReq")); 
	if(0 != rst)
	{
		LOG4CPLUS_ERROR(FLogger, "ckeck command failed. user id = "
			<< pCmd->get_userid() << ", cmd_name = " << pCmd->get_cmd_name());

		return false;
	}

	int game_id  = 0;
	bool bSuccess = decodeParam(pCmd->get_datax(), game_id);
	if(!bSuccess)	
	{
		LOG4CPLUS_ERROR(FLogger, "get game id from datax failed...");
		return false;
	}

	//获取房间配置信息
	vector<GameRoomInfo> room_list;
	GameRoomConfig* pRoomConfig = GameRoomConfig::getInstance();
	pRoomConfig->getRoomConfig(game_id, room_list);

	//获取房间在线人数
	map<RoomIdent, int> rooms_online;
	pRoomConfig->getOnlineInfo(game_id, rooms_online);

	int idx = 0;
	int size = room_list.size();

	IDataX* pParam = new XLDataX();
	XLDataX** rooms_data = new XLDataX*[size];

	RoomIdent room;
	room.game_id = game_id;
	vector<GameRoomInfo>::iterator it = room_list.begin();
	for(; it != room_list.end(); ++it)
	{
		room.room_id = it->room_id;

		rooms_data[idx] = new XLDataX();
		rooms_data[idx]->PutInt(DataID_GameId, it->game_id);
		rooms_data[idx]->PutInt(DataID_RoomId, it->room_id);
		rooms_data[idx]->PutUTF8String(DataID_RoomName, (byte*)it->room_name.c_str(), 
						it->room_name.length());
		rooms_data[idx]->PutInt(DataID_MinBeans_Limit, it->min_limit);
		rooms_data[idx]->PutInt(DataID_MaxBeans_Limit, it->max_limit);
		
		
		rooms_data[idx]->PutInt(DataID_RoomOnlineUsers, rooms_online[room] + tmp_user[idx] );
		rooms_data[idx]->PutUTF8String(DataID_ServerCT, (byte*)it->server_ct.c_str(),
						it->server_ct.length());		
		rooms_data[idx]->PutUTF8String(DataID_ServerCM, (byte*)it->server_cm.c_str(),
						it->server_cm.length());		
		rooms_data[idx]->PutUTF8String(DataID_ServerCU, (byte*)it->server_cu.c_str(),
						it->server_cu.length());		
		rooms_data[idx]->PutInt(DataID_RoomAnte, it->room_ante);
		rooms_data[idx]->PutInt(DataID_QuickMin, it->min_quick);
		rooms_data[idx]->PutInt(DataID_QuickMax, it->max_quick);

		++idx;
	}
	pParam->PutDataXArray(DataID_Param1, (IDataX**)rooms_data, size, true);
	
	delete [] rooms_data;
	rooms_data = NULL;

	DataXCmdPtr pResp(new DataXCmd("GetDirResp", pCmd->get_cipher_flag()));
	pResp->set_datax(pParam);
	pResp->set_userid(pCmd->get_userid());
	
	//send pResp
	CmdTask resp;
	resp.idx = task.idx;
	resp.seqno = task.seqno;
	resp.pCmd = pResp;
	resp.timestamp = task.timestamp;
	ResponseManager::getInstance()->sendResponse(resp);

	return true;
}
Exemple #16
0
bool GoodsHandler::handle(CmdTask& task)
{
	DataXCmdPtr& pCmd = task.pCmd;	

	int rst = checkCmd(pCmd, string("GetStoreConfig")); 
	if(0 != rst)
	{
		LOG4CPLUS_ERROR(FLogger, "ckeck command failed. user id = "
			<< pCmd->get_userid() << ", cmd_name = " << pCmd->get_cmd_name());

		return false;
	}

	int game_id  = 0;
	int tooltype = 0;
	bool bSuccess = decodeParam(pCmd->get_datax(), game_id, tooltype);
	if(!bSuccess)	
	{
		LOG4CPLUS_ERROR(FLogger, "decodeParam failed...");
		return false;
	}

	vector<goods_item> goods_set;

	shop_config::instance()->get_goods_by_type(tooltype, goods_set);	
	LOG4CPLUS_DEBUG(FLogger, "get_goods_by_type = " << goods_set.size());

	int idx = 0;
	int size = goods_set.size();

	IDataX* pParam = new XLDataX();
	XLDataX** datax_goods = new XLDataX*[size];
	
	vector<goods_item>::iterator it = goods_set.begin();
	
	for(; it != goods_set.end(); ++it)
	{		
		datax_goods[idx] = new XLDataX();

		datax_goods[idx]->PutInt(DataID_ToolId, it->toolid);
		datax_goods[idx]->PutUTF8String(DataID_ToolName, (byte*)it->toolname.c_str(), it->toolname.length());
		datax_goods[idx]->PutBytes(DataID_ToolType, (byte*)&(it->tooltype), 1);
		datax_goods[idx]->PutShort(DataID_ToolIcon, it->toolicon);
		datax_goods[idx]->PutInt(DataID_ToolNum, it->toolnum);
		datax_goods[idx]->PutFloat(DataID_Rmb, it->toolrmb);
		datax_goods[idx]->PutFloat(DataID_Keke, it->toolkeke);
		datax_goods[idx]->PutUTF8String(DataID_Content, (byte*)it->content.c_str(), it->content.length());
		datax_goods[idx]->PutBytes(DataID_InStore, (byte*)it->instore.c_str(), it->instore.length());
		datax_goods[idx]->PutBytes(DataID_OutStore, (byte*)it->outstore.c_str(), it->outstore.length());
		datax_goods[idx]->PutShort(DataID_SaleId, it->saleid);
		datax_goods[idx]->PutShort(DataID_TaskId, it->takeid);
		datax_goods[idx]->PutBytes(DataID_VIPValid, (byte*)&(it->vipvalid), 1);
		
		++idx;
	}
	pParam->PutInt(DataID_GameId, 1);
	pParam->PutDataXArray(DataID_Param1, (IDataX**)datax_goods, size, true);

	DataXCmdPtr pResp(new DataXCmd("GetStoreConfigResp", pCmd->get_cipher_flag()));
	pResp->set_datax(pParam);
	pResp->set_userid(pCmd->get_userid());

	delete [] datax_goods;
	datax_goods = NULL;

	CmdTask resp;
	resp.idx = task.idx;
	resp.seqno = task.seqno;
	resp.timestamp = task.timestamp;
	resp.pCmd = pResp;
	ResponseManager::getInstance()->sendResponse(resp);	
	
	return true;
}
Exemple #17
0
int main()
{
	clock_begin();
	Keyboard keyboard;
	usart1.init(12, true);
	// usart1.setTriggerTime(60);
	lcd.init();
	Zigbee zigbee;
	_delay_ms(1000);
	lcd.dis("Connecting...");
	// usart1.send("zigbee_ready(b);");
	// _delay_ms(50);
	while ((addr_b = zigbee.addr()) == 0xfffe)
	{
		_delay_ms(100);
	}
	lcd.dis("Sending...");
	usart1.send("zigbee_ready(b);");
	_delay_ms(50);
	while (!got_addr)
	{
//		usart1.send("zigbee_ready(b);");
		_delay_ms(1000);
		checkCmd(usart1);
	}
	// lcd.dis("Waiting...");
//	usart1.send("zigbee_ready(b);");
	// _delay_ms(50);
// 	while (addr_a == 0xfffe)
// 	{
// 		sprintf(tmp, "Waiting...\n%s\nok", usart1.buf);
// //			usart1.send("zigbee_ready(b);");

// 		lcd.dis(tmp);
// 		checkCmd(usart1);
// 		_delay_ms(1000);
// 	}
	// sprintf(tmp, "Ready.\nADDR_A:%x\nADDR_B:%x", addr_a, addr_b);
	sprintf(tmp, "Ready. \nADDR:%x", addr_b);
	lcd.dis(tmp);
	// usart1.init(0x33, true);
//	usart1.setTrigger(';');

	int keyid;
	while(1)
	{
		if (keyboard.triggered(keyid))
		{
			if (keys[keyid] == 'A')
			{
				mode = 1;
			}
			else if (keys[keyid] == 'B')
			{
				mode = 2;
			}
			else if (keys[keyid] == 'C')
			{
				text[0] = 0;
				lcd.clear();
			}
			else if (keys[keyid] == 'D')
			{
				strcat(text, ";");
				usart1.send(text);
				text[0] = 0;
				lcd.clear();
			}
			else
			{
				switch (mode)
				{
				case 1:
					if (strlen(text) < 64)
					{
						char *p = text;
						for (; *p; ++p);
						*p++ = keys[keyid];
						*p = 0;
						lcd.clear();
						lcd.drawText(0, 0, text);
					}
					break;
				case 2:
					sprintf(tmp, "switch(%d);", keys[keyid]-'0');
//					usart0.send(text);
					usart1.send(tmp);
					// beep(1);
					break;
				}
			}
		}
		checkCmd(usart1);
		_delay_ms(5);
//		_delay_ms(500);
	}
}
void acaUartInterface_thread(void *inContext)
{
  client_log_trace();
  OSStatus err = kUnknownErr;
  arrayent_net_status_t arg;
  arrayent_return_e ret;
  //int8_t retry = 0;
  uint16_t len = 0;
  static char property[150];
  
  mico_rtos_init_semaphore(&_wifiConnected_sem, 1);//初始化信号量
  /* Regisist notifications */
  err = mico_system_notify_register( mico_notify_WIFI_STATUS_CHANGED, (void *)notify_WifiStatusHandler, (void *)inContext);//???
  require_noerr( err, exit );
  while(_wifiConnected == false){//after few time connected wifi success.???
    mico_rtos_get_semaphore(&_wifiConnected_sem, 200000);
    client_log("Waiting for Wi-Fi network...");
    mico_thread_sleep(1);
  }
  user_modules_init();
  OLED_ShowString(OLED_DISPLAY_COLUMN_START, OLED_DISPLAY_ROW_2, "Arrayent        ");
  OLED_ShowString(OLED_DISPLAY_COLUMN_START, OLED_DISPLAY_ROW_3, "  Connecting... ");
  OLED_ShowString(OLED_DISPLAY_COLUMN_START, OLED_DISPLAY_ROW_4, "                ");
  configureArrayent(inContext);
  
  //Initialize Arrayent stack
  client_log("Initializing Arrayent daemon...");
  ret = ArrayentInit();
  if(ret != ARRAYENT_SUCCESS) {
    client_log("Failed to initialize Arrayent daemon.%d", ret);
    return;
  }
  client_log("Arrayent daemon successfully initialized!");
  //Wait for Arrayent to finish connecting to the server
  do {
    ret = ArrayentNetStatus(&arg);//get connecting status to server
    if(ret != ARRAYENT_SUCCESS) {
      client_log("Failed to connect to Arrayent network.");
    }
    client_log("Waiting for good network status.");
    mico_thread_sleep(1);
  } while(!(arg.connected_to_server));//after few time connected server success.
  
  client_log("Connected to Arrayent network!\r\n");
  _connection_lost = false;
  OLED_ShowString(OLED_DISPLAY_COLUMN_START, OLED_DISPLAY_ROW_2, "Connected       ");
  OLED_ShowString(OLED_DISPLAY_COLUMN_START, OLED_DISPLAY_ROW_3, "    Arrayent!   ");
  OLED_ShowString(OLED_DISPLAY_COLUMN_START, OLED_DISPLAY_ROW_4, "                ");
  
  ArrayentSetProperty("DC","0");
  mico_thread_msleep(200);
  ArrayentSetProperty("RGB","off");
  
  err = mico_rtos_create_thread(NULL, MICO_DEFAULT_LIBRARY_PRIORITY, "update", update_property, STACK_SIZE_UART_RECV_THREAD, (void*)inContext);
  require_noerr_action( err, exit, client_log("ERROR: Unable to start the update_property thread.") );
  err = mico_rtos_create_thread(NULL, MICO_APPLICATION_PRIORITY, "except", except_handle, STACK_SIZE_UART_RECV_THREAD, (void*)inContext);
  require_noerr_action( err, exit, client_log("ERROR: Unable to start the except_handle thread.") );
  while(1) {
    //sure wifi connected.
    if(_wifiConnected == false){
      mico_rtos_get_semaphore(&_wifiConnected_sem, 200000);
      continue;
    }
    //sure server connected.
    if(_connection_lost && _wifiConnected){
      do {
        ret = ArrayentNetStatus(&arg);//get connecting status to server
        if(ret != ARRAYENT_SUCCESS) {
          client_log("Failed to connect to Arrayent network.");
        }
        client_log("Waiting for good network status.");
        mico_thread_sleep(1);
      } while(!(arg.connected_to_server));
      _connection_lost = false;
    }
    
    len = sizeof(property);
    if(ARRAYENT_SUCCESS==ArrayentRecvProperty(property,&len,10000)){//receive property data from server
      client_log("property = %s\n\r",property);
      if(0!=checkCmd(property, inContext)){//parse cmd success
        //_response = false;
        //retry = 1;
        client_log("%s", property);
      }
    }
  }
  
exit:
  client_log("Exit: ACA App exit with err = %d", err);
  mico_rtos_delete_thread(NULL);
  return;
}
void debugSM(ros::NodeHandle &n)
{
    //PUBLISHER FOR MOTIONCONTROL
    ros::Publisher roboCom = n.advertise<robot_soccer::controldata>("robot2Com", 5);

    //SUBSCRIBER FROM VISION
    ros::Subscriber vision_subscriber = n.subscribe("vision_data", 5, visionCallback);
    (void*)vision_subscriber;

    ros::Rate loop_rate(TICKS_PER_SEC);
    bool dataInitialized = false;
    bool init = true;

    ros::Subscriber debug_subscriber = n.subscribe("debug", 5, debugCallback);
    (void*)debug_subscriber;
    Skills skill(RobotType::ally2);
    Point dest;
    bool kickball = false;
    char kickCounter = 0;
    bool kickCmd = false;
    while(ros::ok()){

        if (visionUpdated) {
            visionUpdated = false;
            dataInitialized = true;
            field.updateStatus(visionStatus_msg);
        }
        if (newDebugCmd || init) {


            init = false;
            newDebugCmd = false;
            cmdRob2.cmdType = debugCmd.cmdType;
            cmdRob2.x_cmd = debugCmd.x_cmd;
            cmdRob2.y_cmd = debugCmd.y_cmd;
            cmdRob2.theta_cmd = debugCmd.theta_cmd;
            if (cmdRob2.cmdType == "moveslow" || cmdRob2.cmdType == "movefast") {
                moveSpeed speed;
                if(cmdRob2.cmdType == "moveslow"){
                    speed = moveSpeed::slow;
                } else
                    speed = moveSpeed::fast;

                dest = Point(cmdRob2.x_cmd,cmdRob2.y_cmd);
                skill.goToPoint(speed, dest, cmdRob2.theta_cmd);
            } else if (cmdRob2.cmdType == "kick") {
                kickball = true;
                skill.kick();
            } else if (cmdRob2.cmdType == "kickinit"){
                skill.init_kick();
                std::cout << "initing kicker" << std::endl;
            } else if (cmdRob2.cmdType == "kickuninit"){
                std::cout << "unit kicker" << std::endl;
                skill.uninit_kick();
            } else{
                skill.idle();
            }
        }

        skill.tick();
        if (sendCmd_Rob2) {
            sendCmd_Rob2 = false;
            checkCmd(cmdRob2);
            roboCom.publish(cmdRob2);

        }
        ros::spinOnce();

        loop_rate.sleep();
    }
}