bool FileIOManager::readFileToBuffer(std::string filePath, std::vector<unsigned char>& buffer) {
std::ifstream file(filePath, std::ifstream::binary);
if (file.fail()) {
GamaGameEngine::fatalError("Failed to open " + filePath);
return false;
}
file.seekg(0, file.end);
unsigned int fileSize = (unsigned int)file.tellg();
file.seekg(0, file.beg);
fileSize -= (unsigned int)file.tellg();
buffer.resize(fileSize);
logData("Reading file: " + filePath);
if (file)
logData("File read successfully: " + filePath);
else
fatalError("Error reading file: " + filePath);
file.read((char *)&(buffer[0]), fileSize);
file.close();
return true;
}
ccsCOMPL_STAT wsf2ex1APPLICATION::Run()
{
wsf2libLOG_TRACE();
ErrReset();
ccsCOMPL_STAT stat = SUCCESS;
eccsLOG_1(("wsf2ex1 - Entering the main loop."));
while(IsRunning() == ccsTRUE)
{
// I can return on request, such as with EXIT command
// (stat == SUCCESS) or because of an error (stat == FAILURE)
stat = evhHandler->MainLoop();
// In case of error, I try to recover before exiting
if (stat == FAILURE)
{
eccsLOG_1(("A fatal error occurred, try to recover!"));
#if 0
wsf2libASSERTPTR(mEventMgr);
if (mEventMgr->Init() == FAILURE) stat = FAILURE;
wsf2libASSERTPTR(mActionMgr);
if (mActionMgr->Init() == FAILURE) stat = FAILURE;
wsf2libASSERTPTR(mFsm);
if (mFsm->Init() == FAILURE) stat = FAILURE;
#endif
errAdd(wsf2ex1MOD, wsf2ex1ERR_FATAL, __FILE_LINE__, GetProcName());
errCloseStack();
if (stat == FAILURE)
{
eccsLOG_1(("Cannot recover. Application aborted!"));
errAdd(wsf2ex1MOD, wsf2ex1ERR_FATAL, __FILE_LINE__, GetProcName());
errCloseStack();
logData(wsf2ex1MOD, "%s - Application aborted!", GetProcName());
Quit();
}
}
else
{
eccsLOG_1(("wsf2ex1 - Exiting from the main loop. Application quits."));
logData(wsf2ex1MOD, "%s - Application quits.", GetProcName());
Quit();
break;
}
}
return stat;
}
void handleDeviceInput() {
bool throttle = false;
bool dataSent = false;
if ((millis()-throttleMark<1500)) {
throttle=true;
}
switch (device.keyValue()) {
case _LOG_:
logData(device.arguments());
break;
case _TS1_:
if (!throttle) {
dataSent=true;
toPachube(0, device.arguments());
} break;
case _TS2_:
if (!throttle) {
dataSent=true;
toPachube(1, device.arguments());
} break;
case _TS3_:
if (!throttle) {
dataSent=true;
toPachube(2, device.arguments());
} break;
case _TS4_:
if (!throttle) {
dataSent=true;
toPachube(3, device.arguments());
} break;
case _TS5_:
if (!throttle) {
dataSent=true;
toPachube(4, device.arguments());
} break;
case _FAN_:
if (!throttle) {
dataSent=true;
toPachube(5, device.arguments());
} break;
case _CHL_:
if (!throttle) {
dataSent=true;
toPachube(6, device.arguments());
} break;
case _STC_:
char * finger;
if ((finger=strchr(device.arguments(),' ')) != NULL) {
finger[0]='\0';
toPachube(8, finger+1);
}
toPachube(7, device.arguments()); break;
case _FTL_: toPachube(7, device.arguments()); break;
break;
default:
console.printf("%s",device.line());
break;
}
if (dataSent) throttleMark=millis();
}
void Log::logging(LogLevel level, const char * fmt, ...)
{
AutoLock lock(&m_lock);
//先检查日志等级
if (level > m_log_level)
{
return;
}
char buf[MAX_LOG_SIZE-100];
memset(buf, 0, sizeof(buf));
va_list ap;
va_start(ap, fmt);
//vsprintf (buf,fmt, ap);
//truncate,如果超出长度，则截取
vsnprintf_s(buf, sizeof(buf)-1, _TRUNCATE, fmt, ap);
va_end(ap);
logData(level, buf);
//LogMsg log_msg;
//log_msg.level=level;
//strcpy_s(log_msg.data, sizeof(log_msg.data), buf);
//m_queue.append(log_msg);
}
void
logCommand (int command) {
const LogCommandData cmd = {
.command = command
};
logData(LOG_DEBUG, formatLogCommandData, &cmd);
}
void * uart_filPilote_loop(void * arg)
{
char buf[255];
int index=0;
int sampleCount=0;
float currentAcc=0;
int res;
SerialFilPilote();
while(1)
{
res = read(fd_fil_pilote,&buf[index],255);
index+=res;
for(int ii=0;ii<sizeof(buf);ii++)
{
if((buf[ii]=='\n') ||(buf[ii]=='\r') )
{
buf[ii]=0;
index=0;
if(buf[0]=='A')
{
info("AMP","%f",atof(buf+2)/10);
currentAcc+=atof(buf+2);
sampleCount++;
if(sampleCount>=10)
{
// data comming from arduino is 100ma by unit.
// we divide by 600 rather than 60 to get Ampere
sampleCount=0;
logData("amp","house",time(NULL),currentAcc/100.0f);
logData("watt","house",time(NULL),230*currentAcc/100.0f);
currentAcc=0;
}
}
}
}
}
}
void
logTransformedCommand (int oldCommand, int newCommand) {
const LogTransformedCommandData cmd = {
.oldCommand = oldCommand,
.newCommand = newCommand
};
logData(LOG_DEBUG, formatLogTransformedCommandData, &cmd);
}
void
vlogMessage (int level, const char *format, va_list *arguments) {
const LogMessageData msg = {
.format = format,
.arguments = arguments
};
logData(level, formatLogMessageData, &msg);
}
static int writeUnwrittenDiffs(){
int status = updateDb(POLL_INTERVAL, unwrittenDiffs);
logData(unwrittenDiffs);
freeData(unwrittenDiffs);
unwrittenDiffs = NULL;
return status;
}
IOReturn XboxOriginalControllerClass::handleReport(IOMemoryDescriptor * descriptor, IOHIDReportType reportType, IOOptionBits options) {
// IOLog("%s\n", __FUNCTION__);
UInt8 data[sizeof(XBOX360_IN_REPORT)];
if (descriptor->getLength() >= sizeof(XBOX360_IN_REPORT)) {
descriptor->readBytes(0, data, sizeof(XBOX360_IN_REPORT));
const XBOX360_IN_REPORT *report=(const XBOX360_IN_REPORT*)data;
if ((report->header.command==inReport) && (report->header.size==sizeof(XBOX360_IN_REPORT))) {
convertFromXBoxOriginal(data);
if (memcmp(data, lastData, sizeof(XBOX360_IN_REPORT)) == 0) {
repeatCount ++;
// drop triplicate reports
if (repeatCount > 1) {
return kIOReturnSuccess;
}
} else {
repeatCount = 0;
}
memcpy(lastData, data, sizeof(XBOX360_IN_REPORT));
descriptor->writeBytes(0, data, sizeof(XBOX360_IN_REPORT));
//if (data[2]&1) {
// IOLog("%s after %d ", __FUNCTION__, (int)reportType);
// logData(data, 20);
//}
} else {
IOLog("%s %d \n", __FUNCTION__, (int)descriptor->getLength());
IOLog("%s %d \n", __FUNCTION__, (int)reportType);
logData(data, (int)descriptor->getLength());
}
} else {
descriptor->readBytes(0, data, descriptor->getLength());
if (reportType != 0 && data[0] != 0) {
// not a rumble report
IOLog("%s %d \n", __FUNCTION__, (int)reportType);
logData(data, (int)descriptor->getLength());
}
}
IOReturn ret = Xbox360ControllerClass::handleReport(descriptor, reportType, options);
//IOLog("%s END\n", __FUNCTION__);
return ret;
}
void test(volatile long int* char_count)
{
*char_count = 5;
print("char_count = %d\n", (int)*char_count);
logInit();
logData("Main.txt", "This is a Test! %i \n", (int)char_count, char_count);
//playback("Main.txt", 50);
pause(5000);
print("Finished!\n");
}
int main(void)
{
volatile long int char_count = 0;
volatile long int* char_count_pointer = &char_count;
robot_status = ENABLED;
print("Robot Enabled!\n");
pause(1000);
#ifdef TEST
test(char_count_pointer);
return 0;
#endif
#ifdef PLAYBACK //make sure this is before LOG because it will reopen file with write
logInit();
playback("Main.txt", LOG_SIZE);
pause(5000);
#endif
#ifdef LOG
logInit();
logData("Main.txt", "Hello File World!\n", 0, char_count_pointer);
#endif
#ifdef DRIVE
initDrive();
#endif
filter_count = 0;
counter = 0;
print("Hello Robot\n");
low(LED_2);
low(LED_1);
//distance_parallel();
while(1)//main loop
{
debug("loop\n",0);
//debug("Loop\n", 0);
//button();
navigate();
//checkDriveTrain();
//pause(100);
counter++; //VERY IMPORTANT
}
print("Robot disabled!\n");
}
void Log::runInThread()
{
LogMsg log_msg;
try
{
m_queue.remove(&log_msg);
}catch(Exception& err)
{
//timeout
return;
}
logData(log_msg.level, log_msg.data);
}
void *logLoop(void *)
{
set_realtime_priority(10);
if(gVerbose==1)
dbox_printf("_________________Log Thread!\n");
// get time reference
gettimeofday(&logTimeVal, NULL);
logData(0);
logTimeOrig = logTimeVal.tv_usec;
logTimeOrig *= 0.001; // from usec to msec
logTimeOrig += logTimeVal.tv_sec*1000; // from sec to msec
usleep(5000);
while(!gShouldStop)
{
gettimeofday(&logTimeVal, NULL);
unsigned long long currentTime = logTimeVal.tv_usec;
currentTime *= 0.001; // from usec to msec
currentTime += logTimeVal.tv_sec*1000; // from sec to msec
logData(currentTime-logTimeOrig);
usleep(5000);
}
if(logFile.is_open())
logFile.close();
dbox_printf("log thread ended\n");
return (void *)0;
}
void Logger::logData(const char* data, int size, LogType level) {
logData(0, data, size, level);
}
ccsCOMPL_STAT wsf2ex1APPLICATION::Init(int argCount, char *arg[])
{
wsf2libLOG_TRACE();
ErrReset();
// Turn off logging of warning on EXIT command
evhTASK::LogExitWarning(FALSE);
// Parses the command line arguments
// and extract the configuration parameters
// for ENV NAME, DB POINT and PROC NAME from the
// command line or from the enviroment variables
if (EvaluateArgs(argCount, arg, wsf2ex1DB_ROOT_POINT) == FAILURE)
{
errAdd(wsf2ex1MOD, wsf2ex1ERR_INIT, __FILE_LINE__, "wrong argument(s)");
return FAILURE;
}
// Initialize CCS and connect to database
if(InitCCS() == FAILURE)
{
errAdd(wsf2ex1MOD, wsf2ex1ERR_INIT, __FILE_LINE__, "cannot init CCS");
return FAILURE;
}
// Logs current startup configuration
eccsLOG_1(("%s - Application started (proc name: %s, DB root point: %s)",
GetProcName(), GetProcName(), GetDbRoot()));
logData(wsf2ex1MOD,"%s - Application started (proc name: %s, DB root point: %s)",
GetProcName(), GetProcName(), GetDbRoot());
/*
* Create factory for ACTIONS, DATA, CONFIG and CONTROL
*/
wsf2libASSERT(mActionMgr == NULL);
mActionMgr = new wsf2ex1ACTION_MGR(GetDbRoot(), argCount, arg, GetEventMgr());
if (mActionMgr == NULL)
{
errAdd(wsf2ex1MOD, wsf2ex1ERR_FATAL, __FILE_LINE__, "no memory for allocating wsf2ex1ACTION_MGR");
return FAILURE;
}
if (wsf2libHELPER::ObjectOk(mActionMgr, "ACTION_MGR") == ccsFALSE)
{
errAdd(wsf2ex1MOD, wsf2ex1ERR_CREATE, __FILE_LINE__, "wsf2ex1ACTION_MGR");
return FAILURE;
}
/*
* Set FSM context
* TBD: rename method
*/
SetControl(mActionMgr->GetControl());
/*
* Instantiate events and actions
*/
if (CreateEvents(wsf2ex1EVENTS_FILENAME) == FAILURE)
{
errAdd(wsf2ex1MOD, wsf2ex1ERR_CREATE, __FILE_LINE__, "Events");
return FAILURE;
}
if (mActionMgr->CreateActions() == FAILURE)
{
errAdd(wsf2ex1MOD, wsf2ex1ERR_CREATE, __FILE_LINE__, "Actions");
return FAILURE;
}
/*
* Load SCXML Model
*/
if (LoadModel(wsf2ex1MODEL_FILENAME, mActionMgr->SCXMLGetActions(), mActionMgr->SCXMLGetActivities()) == FAILURE)
{
errAdd(wsf2ex1MOD, wsf2ex1ERR_FATAL, __FILE_LINE__, "cannot load SCXML model");
return FAILURE;
}
/*
* Initialize all events and actions and data structures
*/
if (mActionMgr->Init() == FAILURE)
{
errAdd(wsf2ex1MOD, wsf2ex1ERR_FATAL, __FILE_LINE__, "initializing actions and data structures");
return FAILURE;
}
/*
* Start the execution of SCXML model
*/
if (StartModel() == FAILURE)
{
errAdd(wsf2ex1MOD, wsf2ex1ERR_FATAL, __FILE_LINE__, "cannot start SCXML model execution");
return FAILURE;
}
return SUCCESS;
}
void update_capteur_info(char* pBuf)
{
int ii;
int identified=0;
for(ii=0;ii<TH_LAST;ii++)
{
if(pBuf[1]=='V' && thermometer[ii].type == 'V')
{
if(strncmp(pBuf,thermometer[ii].id,18)==0)
{
thermometer[ii].temperature=atof(pBuf+20);
thermometer[ii].mesure_date=time(NULL);
if(strlen(pBuf)==30)
{
rain.current_rain=atoi(pBuf+26);
rain_calcul();
logData("rn",thermometer[ii].name,time(NULL),rain.falled);
info("RF","Received rain %s: %i",thermometer[ii].name,rain.current_rain);
}
identified++;
sem_post(&sem_capteur_data_available);
info("RF","Received thermometer %s: %f",thermometer[ii].name,thermometer[ii].temperature);
logData("th",thermometer[ii].name,time(NULL),thermometer[ii].temperature);
}
}
if(pBuf[1]=='C' && thermometer[ii].type == 'C')
{
if(strncmp(pBuf,thermometer[ii].id,10)==0)
{
// Serial.print("Temp: ");
// int temp = ((data[5] & 0x0F) << 4) | ((data[6] & 0xF0) >> 4);
// Serial.println(temp/10.0f);
//>C:65085033300F30
pBuf[16]=0;
thermometer[ii].temperature=strtol(pBuf+13,0,16)/10.0f;
thermometer[ii].mesure_date=time(NULL);
identified++;
sem_post(&sem_capteur_data_available);
info("RF","Received thermometer %s: %f",thermometer[ii].name,thermometer[ii].temperature);
logData("th",thermometer[ii].name,time(NULL),thermometer[ii].temperature);
pBuf[12]=0;
thermometer[ii].hygrometrie=(float)strtol(pBuf+10,0,16);
info("RF","Received Humidity %s: %f%%",thermometer[ii].name,thermometer[ii].hygrometrie);
logData("hy",thermometer[ii].name,time(NULL),thermometer[ii].hygrometrie);
}
}
}
for(ii=0;ii<IT_LAST;ii++)
{
if(strncmp(pBuf,interrupter[ii].id,10)==0)
{
interrupter[ii].action=(pBuf[11]=='1')? 1 : 0;
interrupter[ii].action_date=time(NULL);
identified++;
sem_post(&sem_capteur_data_available);
info("RF","Received interupter %i: %i",ii,interrupter[ii].action);
}
}
for(ii=0;ii<PR_LAST;ii++)
{
if(strncmp(pBuf,presence[ii].id,10)==0)
{
presence[ii].action_date=time(NULL);
identified++;
sem_post(&sem_capteur_data_available);
info("RF","Received presence : %s",presence[ii].name);
//logData("pr",presence[ii].name,time(NULL),1.0f);
}
}
if(identified==0 && pBuf[0] == '>')
{
warning("RF","Unidentified rf tag %s",pBuf);
}
}
void SkinGroup::run()
{
while(true) {
mutex.wait();
//cout << "Run Thread .......... " << this->groupName << " " << this->taxelDimension << endl;
for (size_t i = 0; i < skinParts.size(); i++) {
yarp::sig::Vector* compensatedData =
skinParts[i]->getCompensatedDataPort()->read();//
//yarp::sig::Vector* compensatedData = (partList[i]->acquireData());
if (compensatedData == NULL) {
cout
<< "Could not get data... Use old data instead. But there might be a problem with the sensors/YARP/CanUSB/etc"
<< endl;
continue;
}
else {
//cout << "OK ---" << compensatedData->size() << "----" << endl;
}
unsigned int offset = offsets[i];
vector<int> sensorIds = skinParts[i]->getAvailableSensorIds();
double *tempData = compensatedData->data();
//cout << "Received data size is ----> " << compensatedData->size() << endl;
for (int j = 0; j < skinParts[i]->getSize(); j++) {
//cout << "j : " << j << " " << sensorIds[j] << " " << sensorIds[j]*NUM_TAXEL_TRIANGLE << " " << offset + j*NUM_TAXEL_TRIANGLE << endl;
if (!tempTactileData->assignData(tempData + sensorIds[j]* NUM_TAXEL_TRIANGLE, offset + j * NUM_TAXEL_TRIANGLE, NUM_TAXEL_TRIANGLE, true)) {
cout << "CANNOT ASSIGN DATA INTO DATASAMPLE.......... " << this->groupName << endl;
}
}
}
tState->insertDataSample(*tempTactileData, false);
if (!tState->updateSalienceMap(TactileState::timeDiff)) {
cerr << "ERROR: CANNOT UPDATE SALIENCE MAP\n";
}
//bool TactileState::updateClassification( methodForClassification &methodClassify)
// int classifyPattern(DataSample &ds, methodForClassification &methodClassify);
/*
if (!tState->updateClassification(TactileState::SVM_RBF))
{
cerr << "ERROR: Cannot perform classification\n";
}
*/
if(this->ifLogData) {
logData(tempTactileData);
}
if (!tempTactileData->aboveThreshold()) {
//cout << "No touch detected " << endl;
//return TactileState::none;
tState->temporalPatternClassify(zjMath::NO_EVENT);
if(!sentNoneEvent) {
if(!this->sendResult("NONE", "NONE")) {
std::cout << "No valid result sent" << std::endl;
}
else {
std::cout << "sent touch class result: NONE" << std::endl;
}
}
sentNoneEvent = true;
}
else {
// only log data when there are events.
/*
if(this->ifLogData)
{
logData(tempTactileData);
}*/
this->classification->prepareSVMNode(*tempTactileData, false);
int touchClass = this->classification->performClassification();
std::string softHard = "NONE";
double maxValue = tempTactileData->max();
/*
if(maxValue > zjMath::THRESHOLD_SOFT_HARD)
{
softHard = "HARD";
}
else
{
softHard = "SOFT";
}
*/
//cout << touchClass << endl;
//cout << "Perform Classification for part: " << this->groupName << ". Predicted label is: " << touchClass << endl;
std::string strClass = tState->temporalPatternClassify(touchClass);
// everything is done here. Need to send result out of port via YARP now.
//std::cout << "Sending out result to port " << this-> classificationOutport.getName().c_str() << std::endl;
//if(!this->sendResult(touchClass))
if(!this->sendResult(strClass, softHard)) {
std::cout << "No valid result sent" << std::endl;
}
else
{
std::cout << "sent touch class result: " << strClass << std::endl;
sentNoneEvent = false;
}
//yarp::os::Time::delay(0.1);
}
mutex.post();
}
}
int main(int argc, char** argv) {
MotionInfoPtr cmd;
int saveStuff = -1;
mcdc_load(); //load mcdc specific structures
settings_init(); //init settings struct
gonz_init(); //init main controller
logging_init();
for (int i=0; i<argc; i++) {
if (strcmp(argv[i],"--test")==0) {
printf("Test Mode\n");
gonz_set_mode(GONZ_MODE_TEST);
}
if (strcmp(argv[i],"--save")==0) {
printf("Saveing Stuff...\n");
saveStuff = 1;
}
}
enableCanEvent(&processCanMsg); //set the can calback
int caninited = can_init(); //init can communication
printf("Can Init Status:%d\n",caninited);
can_startListener(); //start can listener thread
int initialised = 0;
if (gonz_get_mode() == GONZ_MODE_NORMAL) {
printf("Spica Init:\n");
RosHelper::initialize();
}
//RosHelper::sendInfo("Motion Initialiasing...");
do {
initialised = mcdc_init_controllers(); //init the controllers
if (!initialised) {
can_close();
//RosHelper::sendWarning("Cannot Init Controllers");
usleep(500000);
can_init();
}
} while(!initialised);
if(saveStuff > 0) {
printf("Saving...\n");
mcdc_save_all();
sleep(5);
exit(1);
}
//RosHelper::sendInfo("Motion Initialiased.");
//unsigned char buffer[5] = {CAN_CMD_SET_VELOCITY,0,0,0,0};
//INT2BYTEPOS(12*60,buffer,1);
gonz_state.currentMotionGoal.x = 0;
gonz_state.currentMotionGoal.y = 0;
gonz_state.currentMotionGoal.rotation = 0;
while(1) {
//char charbuf[256];
//writeCanMsg(CAN_ID_PDO2_CMD,1,buffer,5);
/*
gonz_state.currentMotionGoal.x = 100;
gonz_state.currentMotionGoal.y = 0;
gonz_state.currentMotionGoal.rotation = 0;
*/
switch(gonz_get_mode()) {
case GONZ_MODE_NORMAL:
cmd = RosHelper::getMotion();
if (cmd) {
// printf("GOT COMMAND\n");
gonz_state.currentMotionGoal.rotation=(cmd->getRotation()*1024);
gonz_state.currentMotionGoal.x=(cmd->getTranslation()*cos(cmd->getAngle()));
gonz_state.currentMotionGoal.y=(cmd->getTranslation()*sin(cmd->getAngle()));
}
printf("MM CURCMD %f\t%f\t%f\n",gonz_state.currentMotionGoal.x,gonz_state.currentMotionGoal.y,gonz_state.currentMotionGoal.rotation);
gonz_main();
break;
case GONZ_MODE_TEST:
gonz_test_loop();
break;
default:
printf("Unknown Mode, doing nothing\n");
}
cout<<"PCanGonzales::main logging data"<<endl;
logData();
mcdc_query_infos();
usleep(current_settings.controllerLoopTime*1000);
}
can_close();
return 0;
}
static void
logSpeechAction (const LogSpeechActionData *lsa) {
logData(LOG_CATEGORY(SPEECH_EVENTS), formatLogSpeechActionData, lsa);
}
void logDataMsg(const char *c, T myArray, const char *c2) {
printf("%s ", c);
logData(myArray);
printf("%s\n", c2);
}
void logDataMsg(const char *c, T myArray) {
printf("%s ", c);
logData(myArray);
printf("\n");
}
void MIDIClient::handleMsg(UInt8 *buf, size_t len) {
MIDIMsgHandler::handleMsg(buf, len);
if(debuggingEnabled)
logData("midiIn", buf, len);
}
void
usbLogSetupPacket (const UsbSetupPacket *setup) {
logData(LOG_CATEGORY(USB_IO), usbFormatLogSetupPacket, setup);
}