void DisplayVector(float vector[3]) //Display array with 3 elements { PrintString("<"); PrintFloat(vector[0]); PrintString(","); PrintFloat(vector[1]); PrintString(","); PrintFloat(vector[2]); PrintString(">"); }
void CAStreamRangedDescription::PrintToLog(const AudioStreamRangedDescription& inDesc) { PrintFloat (" Sample Rate: ", inDesc.mFormat.mSampleRate); PrintFloat (" Max Sample Rate: ", inDesc.mSampleRateRange.mMaximum); PrintFloat (" Min Sample Rate: ", inDesc.mSampleRateRange.mMinimum); Print4CharCode (" Format ID: ", inDesc.mFormat.mFormatID); PrintHex (" Format Flags: ", inDesc.mFormat.mFormatFlags); PrintInt (" Bytes per Packet: ", inDesc.mFormat.mBytesPerPacket); PrintInt (" Frames per Packet: ", inDesc.mFormat.mFramesPerPacket); PrintInt (" Bytes per Frame: ", inDesc.mFormat.mBytesPerFrame); PrintInt (" Channels per Frame: ", inDesc.mFormat.mChannelsPerFrame); PrintInt (" Bits per Channel: ", inDesc.mFormat.mBitsPerChannel); }
void PrintStockInfo(char* ticker, float value, float open, float close, float predict){ int negflag; float change = value - open; if(change < 0){ negflag = 1; change = open - value; } else{ negflag = 0; } setAddrPointer( 0x00 , 0x00 ); PrintString(" Stock Report "); PrintEmptyLine(); PrintEmptyLine(); PrintString(" Ticker: "); PrintString(ticker); int i=0; while(ticker[i++] != '\0'){} while(i++ <= 5){ PrintString(" "); } PrintString(" "); PrintEmptyLine(); PrintString(" Value: "); PrintFloat(value); PrintString(" "); PrintString(" Open: "); PrintFloat(open); PrintString(" "); PrintString(" Close: "); PrintFloat(close); PrintString(" "); //PrintEmptyLine(); PrintString(" Change:"); if(negflag){ PrintString("-"); } else{ PrintString("+"); } PrintFloat(change); PrintString(" "); PrintString(" Predict: "); PrintFloat(predict); PrintString(" "); PrintEmptyLine(); PrintString(" ^ Menu "); PrintString(" <Prev *Print Next> "); PrintString(" v Menu "); PrintEmptyLine(); }
void PrintFullStat(REPORTSTAT *pStat, const char *sStatID, char *sPrintID, FLOAT fValue) { if ((strstr(pStat->sStatParms,sStatID))||(strstr(pStat->sStatParms,"all"))) { PrintString(pRepStats,"%s",pStat->sBaseDescription); PrintString(pRepStats,"_%s = ",sPrintID); PrintFloat(pRepStats,"%f\n",fValue); } }
/* * print constant declarations */ static void PrintConstant(FILE *f, AST *ast) { if (IsFloatConst(ast)) { PrintFloat(f, EvalConstExpr(ast)); return; } PrintExpr(f, ast); }
void Display_Raw(float accel[3], float gyro[3], float temp) //Display raw Accel Gyro and Tempreature Values { PrintString("\nAccel\tTemp\tGyro\t"); for(uint8_t i=0;i<3;i++) { PrintFloat(accel[i]); PrintString("\t"); } PrintFloat(temp); PrintString("\t"); for(uint8_t i=0;i<3;i++) { PrintFloat(gyro[i]); PrintString("\t"); } }
void PrintHdrFloat(REPORT *pRep, unsigned int bCommentChar, const char *sFormat, FLOAT fVal) { FILE *filOut = 0; if (bReportClean) return; if (pRep) filOut = pRep->fileOut; if (bCommentChar) { if (filOut) { fprintf(filOut,"%s ",sCommentString); } if (bReportEcho && (filOut != stdout)) { fprintf(stdout,"%s ",sCommentString); } } PrintFloat(pRep,sFormat,fVal); }
globle void PrintAtom( char *logicalName, int type, void *value) { char buffer[20]; switch (type) { case FLOAT: PrintFloat(logicalName,ValueToDouble(value)); break; case INTEGER: PrintLongInteger(logicalName,ValueToLong(value)); break; case SYMBOL: PrintRouter(logicalName,ValueToString(value)); break; case STRING: if (PreserveEscapedCharacters) { PrintRouter(logicalName,StringPrintForm(ValueToString(value))); } else { PrintRouter(logicalName,"\""); PrintRouter(logicalName,ValueToString(value)); PrintRouter(logicalName,"\""); } break; case EXTERNAL_ADDRESS: if (AddressesToStrings) PrintRouter(logicalName,"\""); PrintRouter(logicalName,"<Pointer-"); sprintf(buffer,"%p",value); PrintRouter(logicalName,buffer); PrintRouter(logicalName,">"); if (AddressesToStrings) PrintRouter(logicalName,"\""); break; #if OBJECT_SYSTEM case INSTANCE_NAME: PrintRouter(logicalName,"["); PrintRouter(logicalName,ValueToString(value)); PrintRouter(logicalName,"]"); break; #endif #if FUZZY_DEFTEMPLATES case FUZZY_VALUE: PrintFuzzyValue(logicalName,ValueToFuzzyValue(value)); break; #endif case RVOID: break; default: if (PrimitivesArray[type] == NULL) break; if (PrimitivesArray[type]->longPrintFunction == NULL) { PrintRouter(logicalName,"<unknown atom type>"); break; } (*PrimitivesArray[type]->longPrintFunction)(logicalName,value); break; } }
globle void PrintAtom( void *theEnv, const char *logicalName, int type, void *value) { struct externalAddressHashNode *theAddress; char buffer[20]; switch (type) { case FLOAT: PrintFloat(theEnv,logicalName,ValueToDouble(value)); break; case INTEGER: PrintLongInteger(theEnv,logicalName,ValueToLong(value)); break; case SYMBOL: EnvPrintRouter(theEnv,logicalName,ValueToString(value)); break; case STRING: if (PrintUtilityData(theEnv)->PreserveEscapedCharacters) { EnvPrintRouter(theEnv,logicalName,StringPrintForm(theEnv,ValueToString(value))); } else { EnvPrintRouter(theEnv,logicalName,"\""); EnvPrintRouter(theEnv,logicalName,ValueToString(value)); EnvPrintRouter(theEnv,logicalName,"\""); } break; case DATA_OBJECT_ARRAY: if (PrintUtilityData(theEnv)->AddressesToStrings) EnvPrintRouter(theEnv,logicalName,"\""); EnvPrintRouter(theEnv,logicalName,"<Pointer-"); gensprintf(buffer,"%p",value); EnvPrintRouter(theEnv,logicalName,buffer); EnvPrintRouter(theEnv,logicalName,">"); if (PrintUtilityData(theEnv)->AddressesToStrings) EnvPrintRouter(theEnv,logicalName,"\""); break; case EXTERNAL_ADDRESS: theAddress = (struct externalAddressHashNode *) value; if (PrintUtilityData(theEnv)->AddressesToStrings) EnvPrintRouter(theEnv,logicalName,"\""); if ((EvaluationData(theEnv)->ExternalAddressTypes[theAddress->type] != NULL) && (EvaluationData(theEnv)->ExternalAddressTypes[theAddress->type]->longPrintFunction != NULL)) { (*EvaluationData(theEnv)->ExternalAddressTypes[theAddress->type]->longPrintFunction)(theEnv,logicalName,value); } else { EnvPrintRouter(theEnv,logicalName,"<Pointer-"); gensprintf(buffer,"%d-",theAddress->type); EnvPrintRouter(theEnv,logicalName,buffer); gensprintf(buffer,"%p",ValueToExternalAddress(value)); EnvPrintRouter(theEnv,logicalName,buffer); EnvPrintRouter(theEnv,logicalName,">"); } if (PrintUtilityData(theEnv)->AddressesToStrings) EnvPrintRouter(theEnv,logicalName,"\""); break; #if OBJECT_SYSTEM case INSTANCE_NAME: EnvPrintRouter(theEnv,logicalName,"["); EnvPrintRouter(theEnv,logicalName,ValueToString(value)); EnvPrintRouter(theEnv,logicalName,"]"); break; #endif case RVOID: break; default: if (EvaluationData(theEnv)->PrimitivesArray[type] == NULL) break; if (EvaluationData(theEnv)->PrimitivesArray[type]->longPrintFunction == NULL) { EnvPrintRouter(theEnv,logicalName,"<unknown atom type>"); break; } (*EvaluationData(theEnv)->PrimitivesArray[type]->longPrintFunction)(theEnv,logicalName,value); break; } }
int computeFS () { uint refresh = 2,key; int index = 0,i,hcs; char ch; double ht,hz,v,bh,hd; double value[4] = {0.0,0.0,0.0,0.0}; const char * context[] = {"","","",""}; while(1){ if (refresh == REFRESH_ALL){ refresh = REFRESH_PART; Bdisp_AllClr_VRAM(); SetMyFont(&stHz12x12,&stAsc6x12); //¶¨Òå×ÖÌå´óС DispStr(2,1, "Ïß "); DispStr(2,12,"· "); DispStr(2,25,"·´ "); DispStr(2,38,"Ëã "); ShowTitle(TITLE); DispStr(17,0, "×ø±êX: "); DispStr(17,13,"×ø±êY: "); DispStr(17,26,"×ø±êZ: "); //DispStr(17,39,"×ø±êZ: "); for (i=0;i<4;++i){ PrintXY(50,2+i*12,(uchar*)context[i],0); } PrintIcon (0,"Rou",0); PrintIcon (1,"Set",0); PrintIcon (2,"Disp",0); PrintIcon (3,"=0=",1); PrintIcon (4,"Save",1); PrintIcon (5,"?",0); } if (refresh == REFRESH_PART){ for (i=0;i<4;++i){ PrintFloat(value[i],48,0+i*12,index==i,4); //ÊäÈë¿òλÖà } refresh = 0; } GetKey(&key); switch(key){ case KEY_CTRL_F1 : SET(); refresh=REFRESH_ALL; break; case KEY_CTRL_F2 : ZSSET(); refresh=REFRESH_ALL; break; case KEY_CTRL_F3 : RestoreDisp(SAVEDISP_PAGE1);refresh=REFRESH_ALL;WaitKey();break; case KEY_CTRL_F4 : case KEY_CTRL_F5 : Nodata(); refresh=REFRESH_ALL; break; case KEY_CTRL_F6 : About(); refresh=REFRESH_ALL; break; case KEY_CTRL_UP : refresh=REFRESH_PART; --index; break; case KEY_CTRL_DOWN : refresh=REFRESH_PART; ++index; break; case KEY_CTRL_EXE: if (index<3){ refresh = REFRESH_PART; index++; }else{ yscs.x1 = value[0]; yscs.y1 = value[1]; hz = value[2]; ht = value[3]; refresh=REFRESH_ALL; //yscs.k=0.0; yscs.k=xy_to_sdSrd( yscs.x1, yscs.y1, 0); yscs.d=xy_to_sdSrd( yscs.x1, yscs.y1, 1); //yscs.k=xy_to_sd( yscs.x1, yscs.y1, 0); //yscs.d=xy_to_sd( yscs.x1, yscs.y1, 1); //FS(); GC(); HP(); if (yscs.d<0) hcs=-1; //Æ«¾à×ó²à if (yscs.d>=0) hcs=1; //Æ«¾àÓÒ²à if (yscs.d*hcs<=zfset.zsp) hd=0; // if (zfset.zsh==0) hd=0; if (yscs.d*hcs>zfset.zsp && yscs.d*hcs-zfset.zsp<zfset.zsh) hd=(yscs.d*hcs-zfset.zsp)*hcs; if (yscs.d*hcs-zfset.zsp>=zfset.zsh) hd=zfset.zsh*hcs; bh=yscs.gch+(yscs.hpd*hd)-ht; Bdisp_AllClr_VRAM(); DispStr(2,0, "¼Æ "); DispStr(2,12,"Ëã "); DispStr(2,26,"½á "); DispStr(2,39,"¹û "); ShowTitle(TITLE); DispStr(17,0,"×®ºÅ "); DispStr(17,13,"Æ«¾à "); DispStr(17,26,"ÌîÍÚ: "); DispStr(17,39,"»ù¸ß: "); PrintfXY (48,2.5,0,"%.4f",yscs.k); PrintfXY (48,15,0,"%.4f",yscs.d); PrintfXY (48,28,0,"%.4f",yscs.gch); PrintfXY (48,42,0,"%.4f",yscs.hpd); //PrintfXY (22,40,0," H= %.3f",bh); //PrintfXY (22,48,0," H-Z= %.3f",hz-bh); SaveDisp(SAVEDISP_PAGE1); WaitKey(); ++index; }break; case KEY_CTRL_EXIT: return 0; default: if ((ch=GetPressNumChar(key))!=0){ char buf[14]; int r; refresh = REFRESH_PART; buf[0] = ch; buf[1] = '\0'; r = InputString (48,0+index*12,buf,0,12); //ÊäÈë¿òλÖà if (r != KEY_CTRL_EXIT){ value[index] = atof(buf); if (r==KEY_CTRL_DOWN) ++index; if (r==KEY_CTRL_UP) --index; if (r==KEY_CTRL_EXE && index<3) ++index; } break; } } if (index >=4) index = 2; if (index < 0) index = 3; } }
int PM_parameter() { char * table_label[]= {"DH","QDZH","ZDZH","X","Y","FWJ","ZX","QDBJ","ZDBJ"}; char * icons[] = {"","","","",""}; char * ch_text[] = {"\xb5\xe3\xc3\xfb", "起点桩号", "终点桩号", "X坐标 ", "Y坐标 ", "方位角 ", "转向 ", "起点半径", "终点半径"}; int x = 0,y = 0,i,j,t; int left = 0,top = 0; int refresh = REDRAW_ALL; int enter,suc; char str[13]=""; char c; PMproduce(); //打开交点参数转换平面参数 InsertLine(l_pqx.length,&y,&refresh); GetListq(); //取链表链表线元参数 while(1){ if (refresh==REDRAW_ALL){ Bdisp_AllClr_VRAM(); for (i=0;i<4;++i) PrintTablesPM(i,0,table_label[left+i],1,1); t = pmroute.size<= 1 ? 2 : 5; //如果交点个数小于等于1 那么T=2 否则等于5 if (t>=2){ SetMyFont(&stHz8x8,&stAsc6x12); DispStr(2,56,"\xCE\xC4\xBC\xFE"); DispStr(23,56,"添加"); } if (t>2){ SetMyFont(&stHz8x8,&stAsc6x12); DispStr(45,56,"插入 "); DispStr(65,56,"删行 "); DispStr(86,56,"清空 "); } for (i=0;i< t;++i) // i<2 或 i<5 PrintIcon(i,icons[i],0); //打印下标 PrintIcon(5,"?",0); //固定打印下标 refresh = REDRAW_PART; // 重画部分 } if (refresh==REDRAW_PART){ //如果画部分 double * sd; for (i=top;i<top+4 && i<pmroute.size;++i){ sd = & (pmroute.set[i].pm_qk); for (j=(left==0 ? 1:0);j<4;++j){ //if (i==0 && j+left>3) ////////////////////////////////////////// //PrintTablesPM(j,i+1-top,"",y==i && x-left==j,0);/////////////// //else///////// PrintTableFormatPM(j,i+1-top,y==i && x-left==j,0,"%lf",sd[j+left-1]); } if (left==0) PrintTableFormatPM(0,i+1-top,y==i && x-left==0,0,"%s",pmroute.set[i].name); PrintTableIndexPM(i-top+1,i); } SetMyFont(&stHz12x12,&stAsc6x12); if(x>0){ //if (!(y==0 && x>2)){ sd = & (pmroute.set[y].pm_qk); //取地址的值*************** DispStr(0,42,ch_text[x]); PrintFloat(sd[x-1],49,42,0,setup_data.point); //打印 //} }else{ DispStr(0,42,ch_text[x]); PrintXY(51,44,pmroute.set[y].name,0); } refresh = 0; } GetKey(&key); switch (key){ case KEY_CTRL_UP: y--;refresh = REDRAW_ALL;break; case KEY_CTRL_DOWN: y++;refresh = REDRAW_ALL;break; case KEY_CTRL_LEFT: x--;refresh = REDRAW_ALL;break; case KEY_CTRL_RIGHT: x++;refresh = REDRAW_ALL;break; case KEY_CTRL_EXIT: pmroute_initial(&pmroute); InitList(&l_pqx); return 0 ; case KEY_CTRL_F1: /* Set */ refresh = REDRAW_ALL; SelectfilePM(); break; case KEY_CTRL_F2: /* Add */ pmroute_push_back (&pmroute); y = pmroute.size - 1; refresh = REDRAW_ALL; break; case KEY_CTRL_F3: /* ins */ //if (y==0) break; pmroute_insert (&pmroute,y); y++; refresh = REDRAW_ALL; break; case KEY_CTRL_F4: /* Del a point */ //if (y==0) break; refresh = REDRAW_ALL; PopUpWin(4);DispStr(25,10,"删 除 一 行?"); locate(3,5);Print("[F1]Yes [F6]No"); GetKey(&key); if (key==KEY_CTRL_F1){ pmroute_del(&pmroute,y);y--; } break; case KEY_CTRL_F5: /* Del All */ refresh = REDRAW_ALL; PopUpWin(4);DispStr(25,10,"清 空 所 有 ?"); locate(3,5);Print("[F1]Yes [F6]No"); GetKey(&key); if (key==KEY_CTRL_F1){ pmroute_initial(&pmroute); pmroute_push_back(&pmroute); } break; case KEY_CTRL_F6: refresh = REDRAW_ALL; //PrintfXY (70,30,0,"%.3f",route.set[0].xy_zr); //DispStr(15,10,"线路名称"); //if(setup_data.pmroute_name[0]==0) //{ //DispStr(25,27,"请保存线路文件 "); //} //else //{ //DispStr(25,27,setup_data.zdmroute_name); //} break; default: /*other things*/ if (x>0 && (c=GetPressNumChar(key))!=0){ char buffer[14]; int r; //if (y==0 && x > 3)break; buffer[0] = c; buffer[1] = '\0'; r = InputString(49,42,buffer,0,12); if (r!=KEY_CTRL_EXIT){ double * sd; sd = (double*)&(pmroute.set[y].pm_qk); sd[x-1] = atof(buffer); switch(r){ case KEY_CTRL_UP: y--;break; case KEY_CTRL_DOWN: y++;break; case KEY_CTRL_EXE: x++;break; } } refresh = REDRAW_ALL; } else if (x==0 && (c=GetPressChar(key))!=0){ char buffer[14]; int r; buffer[0] = c; buffer[1] = '\0'; r = InputString(49,42,buffer,1,12); if (r!=KEY_CTRL_EXIT){ strncpy(pmroute.set[y].name,buffer,12); switch(r){ case KEY_CTRL_UP: y--;break; case KEY_CTRL_DOWN: y++;break; case KEY_CTRL_EXE: x++;break; } } refresh = REDRAW_ALL; } break; } if (x<0) x = 8,y--; if (x>8) x = 0,y++; if (y<0) y = 0; if (y>=pmroute.size) y = pmroute.size - 1; if (y>top + 3) top = y - 3; if (y<top) top = y; if (x>left +3) left = x - 3; if (x<left) left = x; } }
void updateVelocityOutput() { float desiredVelocity = pid.desiredVelocityPID; float P, I, D, currentVelocity; uint32 deltaClocks; CPU_MSR msr; //------Read Encoder and clock msr = DISABLE_INTERRUPTS(); pid.encoderValue = getTicks(); uint32 nowClocks = ClockTime(); RESTORE_INTERRUPTS(msr); //------Time since last function call in seconds uint32 maxClocks = 0xffffffff; if ((nowClocks < pid.lastClockTicks)) deltaClocks = (maxClocks-pid.lastClockTicks)+nowClocks; else deltaClocks = nowClocks - pid.lastClockTicks; float refreshRate = ((float)deltaClocks)/XPAR_CPU_PPC405_CORE_CLOCK_FREQ_HZ ;//time passed since last function call: sec //uint32 refreshRate = (deltaClocks)/(XPAR_CPU_PPC405_CORE_CLOCK_FREQ_HZ/1000) ; //xil_printf("nowClock: %d\r\n", nowClocks); //xil_printf("pid.lastClockTicks: %d\r\n", pid.lastClockTicks); //xil_printf("Delta Clock: %d\r\n", deltaClocks); //xil_printf("Refresh Rate: "); //PrintFloat(refreshRate); //print("\n\r"); pid.lastClockTicks = nowClocks; //------Distance since last function call in ticks int encoderDifference = pid.encoderValue - pid.lastEncoderValue; //xil_printf("Encoder Value: %d\r\n", pid.encoderValue); //xil_printf("Last Encoder Value: %d\r\n", pid.lastEncoderValue); //xil_printf("Encoder Diff: %d\r\n", encoderDifference); //------Calculate velocity currentVelocity = ((encoderDifference) / (refreshRate)); //xil_printf("Current Velocity: "); PrintFloat(currentVelocity); xil_printf(","); //xil_printf("Desired Velocity: "); PrintFloat(desiredVelocity); xil_printf(","); //print("\n\r"); //------Error pid.error = desiredVelocity - (currentVelocity); // xil_printf("error: "); // PrintFloat(pid.error); // print("\n\r"); //------Update Derivative pid.differentiator = (2*pid.Tau-refreshRate)/(2*pid.Tau+refreshRate)*pid.differentiator + 2/(2*pid.Tau+refreshRate)*(pid.error-pid.lastError); //xil_printf("differentiator: "); //PrintFloat(pid.differentiator); //print("\n\r"); //------Update integrator - AntiWindup(only use the integrator if we are close, but not too close) if ((pid.error < 1500 && pid.error > -1500) && (pid.error > 10 || pid.error < -10)){ pid.integrator = pid.integrator + (refreshRate/2)*(pid.error + pid.lastError); } else { pid.integrator = 0; } // if(pid.error == 0) { // pid.integrator = 0; // } // xil_printf("integrator: "); // PrintFloat(pid.integrator); // print("\n\r"); //------Output Calculation P = pid.Kp * pid.error; I = pid.Ki * pid.integrator; D = pid.Kd * 0;//pid.differentiator; pid.outputPID_unsat = P + I - D; pid.outputPID = sat(pid.outputPID_unsat, 60); //------Save states and send PWM to motors pid.lastEncoderValue = pid.encoderValue; pid.lastCurrentVelocity = currentVelocity; pid.lastDesiredVelocity = desiredVelocity; SetServo(RC_VEL_SERVO, (int)pid.outputPID); // xil_printf("Output PID unsat: "); // PrintFloat(pid.outputPID_unsat); // print("\n\r"); // xil_printf("Output PID: %d", pid.outputPID); // print("--------------------------------------\r\n"); }
globle long int EnvRun( void *theEnv, long int runLimit) { long int rulesFired = 0; DATA_OBJECT result; struct callFunctionItem *theRunFunction; #if DEBUGGING_FUNCTIONS unsigned long maxActivations = 0, sumActivations = 0; #if DEFTEMPLATE_CONSTRUCT unsigned long maxFacts = 0, sumFacts = 0; #endif #if OBJECT_SYSTEM unsigned long maxInstances = 0, sumInstances = 0; #endif double endTime, startTime = 0.0; unsigned long tempValue; #endif unsigned int i; struct patternEntity *theMatchingItem; struct partialMatch *theBasis; ACTIVATION *theActivation; char *ruleFiring; #if PROFILING_FUNCTIONS struct profileFrameInfo profileFrame; #endif /*=====================================================*/ /* Make sure the run command is not already executing. */ /*=====================================================*/ if (EngineData(theEnv)->AlreadyRunning) return(0); EngineData(theEnv)->AlreadyRunning = TRUE; /*================================*/ /* Set up statistics information. */ /*================================*/ #if DEBUGGING_FUNCTIONS if (EngineData(theEnv)->WatchStatistics) { #if DEFTEMPLATE_CONSTRUCT maxFacts = GetNumberOfFacts(theEnv); sumFacts = maxFacts; #endif #if OBJECT_SYSTEM maxInstances = GetGlobalNumberOfInstances(theEnv); sumInstances = maxInstances; #endif maxActivations = GetNumberOfActivations(theEnv); sumActivations = maxActivations; startTime = gentime(); } #endif /*=============================*/ /* Set up execution variables. */ /*=============================*/ if (EvaluationData(theEnv)->CurrentEvaluationDepth == 0) SetHaltExecution(theEnv,FALSE); EngineData(theEnv)->HaltRules = FALSE; /*=====================================================*/ /* Fire rules until the agenda is empty, the run limit */ /* has been reached, or a rule execution error occurs. */ /*=====================================================*/ theActivation = NextActivationToFire(theEnv); while ((theActivation != NULL) && (runLimit != 0) && (EvaluationData(theEnv)->HaltExecution == FALSE) && (EngineData(theEnv)->HaltRules == FALSE)) { /*===========================================*/ /* Detach the activation from the agenda and */ /* determine which rule is firing. */ /*===========================================*/ DetachActivation(theEnv,theActivation); ruleFiring = EnvGetActivationName(theEnv,theActivation); theBasis = (struct partialMatch *) GetActivationBasis(theActivation); EngineData(theEnv)->ExecutingRule = (struct defrule *) GetActivationRule(theActivation); /*=============================================*/ /* Update the number of rules that have fired. */ /*=============================================*/ rulesFired++; if (runLimit > 0) { runLimit--; } /*==================================*/ /* If rules are being watched, then */ /* print an information message. */ /*==================================*/ #if DEBUGGING_FUNCTIONS if (EngineData(theEnv)->ExecutingRule->watchFiring) { char printSpace[60]; sprintf(printSpace,"FIRE %4ld ",rulesFired); EnvPrintRouter(theEnv,WTRACE,printSpace); EnvPrintRouter(theEnv,WTRACE,ruleFiring); EnvPrintRouter(theEnv,WTRACE,": "); PrintPartialMatch(theEnv,WTRACE,theBasis); EnvPrintRouter(theEnv,WTRACE,"\n"); } #endif /*=================================================*/ /* Remove the link between the activation and the */ /* completed match for the rule. Set the busy flag */ /* for the completed match to TRUE (so the match */ /* upon which our RHS variables are dependent is */ /* not deleted while our rule is firing). Set up */ /* the global pointers to the completed match for */ /* routines which do variable extractions. */ /*=================================================*/ theBasis->binds[theBasis->bcount].gm.theValue = NULL; theBasis->busy = TRUE; EngineData(theEnv)->GlobalLHSBinds = theBasis; EngineData(theEnv)->GlobalRHSBinds = NULL; /*===================================================================*/ /* Increment the count for each of the facts/objects associated with */ /* the rule activation so that the facts/objects cannot be deleted */ /* by garbage collection while the rule is executing. */ /*===================================================================*/ for (i = 0; i < theBasis->bcount; i++) { theMatchingItem = theBasis->binds[i].gm.theMatch->matchingItem; if (theMatchingItem != NULL) { (*theMatchingItem->theInfo->incrementBasisCount)(theEnv,theMatchingItem); } } /*====================================================*/ /* Execute the rule's right hand side actions. If the */ /* rule has logical CEs, set up the pointer to the */ /* rules logical join so the assert command will */ /* attach the appropriate dependencies to the facts. */ /*====================================================*/ EngineData(theEnv)->TheLogicalJoin = EngineData(theEnv)->ExecutingRule->logicalJoin; EvaluationData(theEnv)->CurrentEvaluationDepth++; SetEvaluationError(theEnv,FALSE); EngineData(theEnv)->ExecutingRule->executing = TRUE; #if PROFILING_FUNCTIONS StartProfile(theEnv,&profileFrame, &EngineData(theEnv)->ExecutingRule->header.usrData, ProfileFunctionData(theEnv)->ProfileConstructs); #endif EvaluateProcActions(theEnv,EngineData(theEnv)->ExecutingRule->header.whichModule->theModule, EngineData(theEnv)->ExecutingRule->actions,EngineData(theEnv)->ExecutingRule->localVarCnt, &result,NULL); #if PROFILING_FUNCTIONS EndProfile(theEnv,&profileFrame); #endif EngineData(theEnv)->ExecutingRule->executing = FALSE; SetEvaluationError(theEnv,FALSE); EvaluationData(theEnv)->CurrentEvaluationDepth--; EngineData(theEnv)->TheLogicalJoin = NULL; /*=====================================================*/ /* If rule execution was halted, then print a message. */ /*=====================================================*/ #if DEBUGGING_FUNCTIONS if ((EvaluationData(theEnv)->HaltExecution) || (EngineData(theEnv)->HaltRules && EngineData(theEnv)->ExecutingRule->watchFiring)) #else if ((EvaluationData(theEnv)->HaltExecution) || (EngineData(theEnv)->HaltRules)) #endif { PrintErrorID(theEnv,"PRCCODE",4,FALSE); EnvPrintRouter(theEnv,WERROR,"Execution halted during the actions of defrule "); EnvPrintRouter(theEnv,WERROR,ruleFiring); EnvPrintRouter(theEnv,WERROR,".\n"); } /*===================================================================*/ /* Decrement the count for each of the facts/objects associated with */ /* the rule activation. If the last match for the activation */ /* is from a not CE, then we need to make sure that the last */ /* match is an actual match for the CE and not a counter. */ /*===================================================================*/ theBasis->busy = FALSE; for (i = 0; i < (theBasis->bcount - 1); i++) { theMatchingItem = theBasis->binds[i].gm.theMatch->matchingItem; if (theMatchingItem != NULL) { (*theMatchingItem->theInfo->decrementBasisCount)(theEnv,theMatchingItem); } } i = (unsigned) (theBasis->bcount - 1); if (theBasis->counterf == FALSE) { theMatchingItem = theBasis->binds[i].gm.theMatch->matchingItem; if (theMatchingItem != NULL) { (*theMatchingItem->theInfo->decrementBasisCount)(theEnv,theMatchingItem); } } /*========================================*/ /* Return the agenda node to free memory. */ /*========================================*/ RemoveActivation(theEnv,theActivation,FALSE,FALSE); /*======================================*/ /* Get rid of partial matches discarded */ /* while executing the rule's RHS. */ /*======================================*/ FlushGarbagePartialMatches(theEnv); /*==================================*/ /* Get rid of other garbage created */ /* while executing the rule's RHS. */ /*==================================*/ PeriodicCleanup(theEnv,FALSE,TRUE); /*==========================*/ /* Keep up with statistics. */ /*==========================*/ #if DEBUGGING_FUNCTIONS if (EngineData(theEnv)->WatchStatistics) { #if DEFTEMPLATE_CONSTRUCT tempValue = GetNumberOfFacts(theEnv); if (tempValue > maxFacts) maxFacts = tempValue; sumFacts += tempValue; #endif #if OBJECT_SYSTEM tempValue = GetGlobalNumberOfInstances(theEnv); if (tempValue > maxInstances) maxInstances = tempValue; sumInstances += tempValue; #endif tempValue = GetNumberOfActivations(theEnv); if (tempValue > maxActivations) maxActivations = tempValue; sumActivations += tempValue; } #endif /*==================================*/ /* Update saliences if appropriate. */ /*==================================*/ if (EnvGetSalienceEvaluation(theEnv) == EVERY_CYCLE) EnvRefreshAgenda(theEnv,NULL); /*========================================*/ /* Execute the list of functions that are */ /* to be called after each rule firing. */ /*========================================*/ for (theRunFunction = EngineData(theEnv)->ListOfRunFunctions; theRunFunction != NULL; theRunFunction = theRunFunction->next) { if (theRunFunction->environmentAware) { (*theRunFunction->func)(theEnv); } else { ((void (*)(void))(*theRunFunction->func))(); } } /*========================================*/ /* If a return was issued on the RHS of a */ /* rule, then remove *that* rule's module */ /* from the focus stack */ /*========================================*/ if (ProcedureFunctionData(theEnv)->ReturnFlag == TRUE) { RemoveFocus(theEnv,EngineData(theEnv)->ExecutingRule->header.whichModule->theModule); } ProcedureFunctionData(theEnv)->ReturnFlag = FALSE; /*========================================*/ /* Determine the next activation to fire. */ /*========================================*/ theActivation = (struct activation *) NextActivationToFire(theEnv); /*==============================*/ /* Check for a rule breakpoint. */ /*==============================*/ if (theActivation != NULL) { if (((struct defrule *) GetActivationRule(theActivation))->afterBreakpoint) { EngineData(theEnv)->HaltRules = TRUE; EnvPrintRouter(theEnv,WDIALOG,"Breaking on rule "); EnvPrintRouter(theEnv,WDIALOG,EnvGetActivationName(theEnv,theActivation)); EnvPrintRouter(theEnv,WDIALOG,".\n"); } } } /*=====================================================*/ /* Make sure run functions are executed at least once. */ /*=====================================================*/ if (rulesFired == 0) { for (theRunFunction = EngineData(theEnv)->ListOfRunFunctions; theRunFunction != NULL; theRunFunction = theRunFunction->next) { if (theRunFunction->environmentAware) { (*theRunFunction->func)(theEnv); } else { ((void (*)(void))(*theRunFunction->func))(); } } } /*======================================================*/ /* If rule execution was halted because the rule firing */ /* limit was reached, then print a message. */ /*======================================================*/ if (runLimit == rulesFired) { EnvPrintRouter(theEnv,WDIALOG,"rule firing limit reached\n"); } /*==============================*/ /* Restore execution variables. */ /*==============================*/ EngineData(theEnv)->ExecutingRule = NULL; EngineData(theEnv)->HaltRules = FALSE; /*=================================================*/ /* Print out statistics if they are being watched. */ /*=================================================*/ #if DEBUGGING_FUNCTIONS if (EngineData(theEnv)->WatchStatistics) { char printSpace[60]; endTime = gentime(); PrintLongInteger(theEnv,WDIALOG,rulesFired); EnvPrintRouter(theEnv,WDIALOG," rules fired"); #if (! GENERIC) if (startTime != endTime) { EnvPrintRouter(theEnv,WDIALOG," Run time is "); PrintFloat(theEnv,WDIALOG,endTime - startTime); EnvPrintRouter(theEnv,WDIALOG," seconds.\n"); PrintFloat(theEnv,WDIALOG,(double) rulesFired / (endTime - startTime)); EnvPrintRouter(theEnv,WDIALOG," rules per second.\n"); } else { EnvPrintRouter(theEnv,WDIALOG,"\n"); } #endif #if DEFTEMPLATE_CONSTRUCT sprintf(printSpace,"%ld mean number of facts (%ld maximum).\n", (long) (((double) sumFacts / (rulesFired + 1)) + 0.5), maxFacts); EnvPrintRouter(theEnv,WDIALOG,printSpace); #endif #if OBJECT_SYSTEM sprintf(printSpace,"%ld mean number of instances (%ld maximum).\n", (long) (((double) sumInstances / (rulesFired + 1)) + 0.5), maxInstances); EnvPrintRouter(theEnv,WDIALOG,printSpace); #endif sprintf(printSpace,"%ld mean number of activations (%ld maximum).\n", (long) (((double) sumActivations / (rulesFired + 1)) + 0.5), maxActivations); EnvPrintRouter(theEnv,WDIALOG,printSpace); } #endif /*==========================================*/ /* The current module should be the current */ /* focus when the run finishes. */ /*==========================================*/ if (EngineData(theEnv)->CurrentFocus != NULL) { if (EngineData(theEnv)->CurrentFocus->theModule != ((struct defmodule *) EnvGetCurrentModule(theEnv))) { EnvSetCurrentModule(theEnv,(void *) EngineData(theEnv)->CurrentFocus->theModule); } } /*===================================*/ /* Return the number of rules fired. */ /*===================================*/ EngineData(theEnv)->AlreadyRunning = FALSE; return(rulesFired); }
/** Print out a matrix to the GUI or command line */ void mPrint(Mat* m, char* msg, int inv) { #ifndef SIMULATOR { int i,j; if(m == NULL || msg == NULL) return;// MAT_ERROR_NULL_PTR; Println(); if(msg != NULL) { Print(msg); if(m->cols > 1) Println(); } for(i=0 ; i < m->rows ; i++) { for(j=0 ; j < m->cols ; j++) { if(m->type == MAT_32F || m->type == MAT_64F) { PrintFloat(mGet(m,i,j)); Print("\t"); } else { PrintFloat(mGetu(m,i,j)); Print("\t"); } } if(m->cols > 1) Println(); } if(m->cols == 1) Println(); } #endif #ifdef SIMULATOR { int i,j,iend,jend; printf("\r\n"); iend = (inv == 0) ? m->rows : m->cols; jend = (inv == 0) ? m->cols : m->rows; if(msg != NULL) { printf(msg); if(jend > 1) printf("\r\n"); } for(i = 0 ; i < iend ; i++) { for(j = 0 ; j < jend ; j++) { if(inv == 0) printf("%16.5f\t",mGet(m,i,j)); else printf("%16.5f\t",mGet(m,j,i)); } if(jend > 1) printf("\r\n"); } if(jend == 1) printf("\r\n"); } #endif }
int main() { SerialDebug(250000); //PrintString() and PrintFloat() using UART BeginBasics(); Blink(); Enable_PeriphClock(); /*Roll-0 Pitch-1 Yaw-2..Roll rotation around X axis.Pitch rotation around Y axis and Yaw rotation around Z axis note: It does not mean rotation along the X Y or Z axis*/ float RPY_c[3],RPY_k[3]; //RPY_c and RPY_k..Roll pitch and yaw obtained from complemntary filter and kalman filter float Accel[3],Gyro[3],Tempreature; //raw values float Accel_RealWorld[3]; //Real World Acceleration while(Init_I2C(400)){PrintString("\nI2C Connection Error");} while(MPU6050_Init()){PrintString("MPU6050 Initialization Error");} MPU6050_UpdateOffsets(&MPU6050_Offsets[0]); //MPU6050_ConfirmOffsets(&MPU6050_Offsets[0]); while(0) //to play around with quaternions and vector rotation { float vector[3]= {1,0,0}; float axis_vector[3]= {0,1,0}; //rotate around Y axis float rot_angle=90; //with 90 degrees Quaternion q; q=RotateVectorY(vector,rot_angle); //Rotates vector around Y axis with rot_angle PrintString("\nRotated Vector's Quaternion\t"); DisplayQ(q); PrintString("\n"); } while(1) { MPU6050_GetRaw(&Accel[0],&Gyro[0],&Tempreature); //Reads MPU6050 Raw Data Buffer..i.e Accel Gyro and Tempreature values if(Gyro[2]<0.3 && Gyro[2]>-0.3) Gyro[2]=0; //this actually reduces Yaw drift..will add magnetometer soon spudnut=tics(); //tics() return current timing info..using SysTick running at CPU_Core_Frequency/8..Counter Runs from 0xFFFFFF to 0 therefore overflows every 1.864135 secs delt=spudnut-donut; //small time dt donut=spudnut; //Display_Raw(Accel,Gyro,Tempreature); Attitude_k(Accel,Gyro,RPY_k,delt); //Estimates YPR using Kalman PrintString("\nYPR\t"); PrintFloat(RPY_k[2]); PrintString("\t"); PrintFloat(RPY_k[1]); PrintString("\t"); PrintFloat(RPY_k[0]); RemoveGravity(RPY_k,Accel,Accel_RealWorld); PrintString("\tReal World Accel with Gravity\t"); //still glitchish..working on it DisplayVector(Accel_RealWorld); PrintString("\t"); PrintFloat(1/delt); } }
void PrintStats() { unsigned int j,k; unsigned int iRow; REPORTCOL *pCol; REPORTSTAT *pStat; FLOAT fMean; FLOAT fMedian; FLOAT fStdDev; FLOAT fTemp; Print(pRepStats,"\n\n"); for (k=0;k<iNumStatsActive;k++) { for (j=0;j<listStats.iNumItems;j++) { pStat = &aStats[j]; if ((pStat->bActive)&&(pStat->iActiveID==k)) { if (pStat->bCustomField) { switch (pStat->eCustomType) { case ColInt: PrintUInt(pRepStats,pStat->sPrintCustomFormat,*(unsigned int *) pStat->pCustomValue); break; case ColFloat: PrintFloat(pRepStats,pStat->sPrintCustomFormat,*(FLOAT *) pStat->pCustomValue); break; } } else { pCol = &aColumns[FindItem(&listColumns,pStat->sDataColumn)]; if (iRun) { fMean = 0.0; for (iRow = 0; iRow < iRun; iRow++) { fMean += GetRowElement(pCol,iRow); } fMean /= (FLOAT) iRun; fStdDev = 0.0; if (iRun > 1) { for (iRow = 0; iRow < iRun; iRow++) { fTemp = GetRowElement(pCol,iRow)-fMean; fStdDev += (fTemp*fTemp); } fStdDev /= (FLOAT) (iRun-1); fStdDev = sqrt(fStdDev); } fMedian = GetRowElement(pCol,aSortedBySteps[(iRun-1)>>1]); if (iRun % 2 == 0) { fMedian += GetRowElement(pCol,aSortedBySteps[(iRun)>>1]); fMedian /= 2.0; } PrintFullStat(pStat,"mean","Mean",fMean); PrintFullStat(pStat,"stddev","StdDev",fStdDev); PrintFullStat(pStat,"cv","CoeffVariance",fStdDev / fMean); PrintFullStat(pStat,"median","Median",fMedian); PrintFullStat(pStat,"min","Min",GetRowElement(pCol,aSortedBySteps[0])); PrintFullStat(pStat,"max","Max",GetRowElement(pCol,aSortedBySteps[iRun-1])); PrintFullStat(pStat,"q05","Q.05",GetRowElement(pCol,aSortedBySteps[(int)(floor(0.05 * (FLOAT) (iRun-1)))])); PrintFullStat(pStat,"q10","Q.10",GetRowElement(pCol,aSortedBySteps[(int)(floor(0.10 * (FLOAT) (iRun-1)))])); PrintFullStat(pStat,"q25","Q.25",GetRowElement(pCol,aSortedBySteps[(int)(floor(0.25 * (FLOAT) (iRun-1)))])); PrintFullStat(pStat,"q75","Q.75",GetRowElement(pCol,aSortedBySteps[(int)(floor(0.75 * (FLOAT) (iRun-1)))])); PrintFullStat(pStat,"q90","Q.90",GetRowElement(pCol,aSortedBySteps[(int)(floor(0.90 * (FLOAT) (iRun-1)))])); PrintFullStat(pStat,"q95","Q.95",GetRowElement(pCol,aSortedBySteps[(int)(floor(0.95 * (FLOAT) (iRun-1)))])); PrintFullStat(pStat,"q98","Q.98",GetRowElement(pCol,aSortedBySteps[(int)(floor(0.98 * (FLOAT) (iRun-1)))])); PrintFullStat(pStat,"qr75/25","Q.75/25",GetRowElement(pCol,aSortedBySteps[(int)(floor(0.75 * (FLOAT) (iRun-1)))])/GetRowElement(pCol,aSortedBySteps[(int)(floor(0.25 * (FLOAT) (iRun-1)))])); PrintFullStat(pStat,"qr90/10","Q.90/10",GetRowElement(pCol,aSortedBySteps[(int)(floor(0.90 * (FLOAT) (iRun-1)))])/GetRowElement(pCol,aSortedBySteps[(int)(floor(0.10 * (FLOAT) (iRun-1)))])); PrintFullStat(pStat,"qr95/05","Q.95/05",GetRowElement(pCol,aSortedBySteps[(int)(floor(0.95 * (FLOAT) (iRun-1)))])/GetRowElement(pCol,aSortedBySteps[(int)(floor(0.05 * (FLOAT) (iRun-1)))])); } } } } }