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)))]));
}
}
}
}
}
