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);
}
}
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();
}
}
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"));
}
}
}
}
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();
}
}
/*
* 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);
}
}
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);
}
}
}
}
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;
}
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;
}
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;
}
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();
}
}