void AjustaVW(int SpeedDreta, int SpeedEsquerra) { float radi; if (SpeedDreta != SpeedEsquerra) { // Hi ha velocitat angular radi = 2*RADI_COTXE*SpeedEsquerra / (float)(SpeedDreta-SpeedEsquerra); if (radi > RADI_COTXE) W = GetV(SpeedDreta)/radi; else W = GetW(SpeedDreta)/2; } else W = 0; V = GetV(SpeedDreta); }
double V_QChem::GetV(vector<Particle> part, Propagator * rho){ double V = 0.0; V += GetV(part); V += rho->ModifyPotential(part); return V; }
double V_UCHO::GetV(vector<Particle> part, Propagator * rho) { double V = 0; V += GetV(part); // cout << "V: " << V << endl; V += rho->ModifyPotential(part); // cout << "V: " << V << endl; return V; }
//The function // //Starts engine (gets it from min speed to max speed) // float Start(float V, float dt, float Accel, float Vmax) { while(V<Vmax) { //OCR=GetFrequency(V);//send 1 dt seconds with GetFrequency(V) frequency V=GetV(V,dt,Accel);//get current V //delayms(dt); } return Vmax; }
unsigned int MtGetTimeFromDistance(int Distance, int Speed) { // Pre: Distance en centímetres i positiu. // Pre: Speed entre -100 i 100, però valors per sota de 20 retornen zero // Post: Retorna el nombre de milisegons que cal donar a la rutina LS_MT_Lineal per // tal que el robot avançi el nombre de centímetres expressats a Distance, a la velocitat // expressada per Speed. El punt òptim és dona quan la tensió de bateria està entre 7.2 i 7.1, // fora d'aquest marges, l'error pot ser del 10% double t = GetV(Speed); // en centímetres per segon if (t == 0) return 0; t = Distance / t; // de centímetres a segons return t * 1000.0; // de segons a milisegons }
float Stop(float V, float dt, float Accel,float KurrPhase,float GivPhase) { float StopPhase=V*(1 + V / Accel); do { if(GivPhase-KurrPhase>StopPhase||GivPhase-KurrPhase<0) { //send 1 dt seconds with GetFrequency(V) frequency } else { V=GetV(V,dt,-Accel); //OCR=GetFrequency(V);//send 1 dt seconds with GetFrequency(V) frequency } //delayms(dt); KurrPhase=GetPhase(KurrPhase,dt,V); }while(V>0); return 0; }
void sendCommMessage(void){ if(EnableSensorFeedbackMessages){ UARTprintf("(: %d %d %d %d %d%.3d %d :)\r\n", GetLeftEncoder(), GetRightEncoder(), GetV(), GetW(), GetTime(),GetMS(), GetMessageRate() ); //UARTprintf("^.- ROLL %d PTCH %d YAWW %d -.^\r\n", roll, pitch, yaw ); /*UARTprintf("(: %d %d %d %d %d %d %d %d%.3d %d :)\r\n", GetLeftEncoder(), GetRightEncoder(), GetV(), GetW(), GetRoll(), GetPitch(), GetYaw(), GetTime(), GetMS(), GetMessageRate() );*/ } }
void ExecutaOrdre(void) { switch (OrdreAct.Opcode) { case M_LIN: MtSetPWM(M_ESQUERRA, GetVelocitatEsquerra(), GetDireccio()); MtSetPWM(M_DRET, GetVelocitatDreta(), GetDireccio()); if (GetDireccio() == M_DIR_ENDARRERA) V = -GetV(GetVelocitatDreta()); else V = GetV(GetVelocitatDreta()); W = 0; // LS_USB_printf("LIN V = %f, W = %f\n\r", V, W); break; case M_GD: // Si el radi és menor que la meitat de la distància entre rodes, cal oposar els motors // Si no, sempre vas endavant if (OrdreAct.Radi < RADI_COTXE) { MtSetPWM(M_ESQUERRA, GetRadi(), M_DIR_ENDAVANT); MtSetPWM(M_DRET, GetRadi(), M_DIR_ENDARRERA); W = -GetW(GetRadi()); V = 0; // LS_USB_printf("GD V = %f, W = %f\n\r", V, W); } else { MtSetPWM(M_ESQUERRA, GetVelocitatEsquerra(), GetDireccio()); MtSetPWM(M_DRET, GetRadiInterior(GetVelocitatDreta()), GetDireccio()); if (GetDireccio() == M_DIR_ENDARRERA) V = -GetV(GetVelocitatEsquerra()); else V = GetV(GetVelocitatEsquerra()); NvSetOmega(GetVelocitatEsquerra(), GetDireccio(), GetRadiInterior(GetVelocitatDreta()), GetDireccio()); } break; case M_GE: // Si el radi és menor que la meitat de la distància entre rodes, cal oposar els motors // Si no, sempre vas endavant if (OrdreAct.Radi < RADI_COTXE) { MtSetPWM(M_ESQUERRA, GetRadi(), M_DIR_ENDARRERA); MtSetPWM(M_DRET, GetRadi(), M_DIR_ENDAVANT); W = GetW(GetRadi()); V = 0; // LS_USB_printf("GE V = %f, W = %f\n\r", V, W); } else { MtSetPWM(M_ESQUERRA, GetRadiInterior(GetVelocitatEsquerra()), GetDireccio()); MtSetPWM(M_DRET, GetVelocitatDreta(), GetDireccio()); if (GetDireccio() == M_DIR_ENDARRERA) V = -GetV(GetVelocitatDreta()); else V = GetV(GetVelocitatDreta()); NvSetOmega(GetRadiInterior(GetVelocitatEsquerra()), GetDireccio(), GetVelocitatDreta(), GetDireccio()); } break; } }
INLINE BOOL LoadTaskRegDatabase( VOID ) { D_S(struct FileInfoBlock,fib); BOOL rc = TRUE; STRPTR file; ENTER(); if((file = FileToMem( DATABASE_FILE, fib, DOSBase ))) transcode( file, fib->fib_Size ); if(file == NULL) { LONG ioerr; if((ioerr = IoErr()) != ERROR_OBJECT_NOT_FOUND) { ShowIoErrMessage("cannot load database"); rc = FALSE; } } else if(fib->fib_Size < (LONG)sizeof(ULONG) || *((ULONG *)file) != DATABASE_ID) { if((rc=CorruptDatabase())) { FreeVec( file ); file = NULL; } } if( rc == TRUE && file != NULL ) { STRPTR ptr=&file[sizeof(ULONG)]; LONG size=(fib->fib_Size)-sizeof(ULONG), error = 0; // this is version 1, hence no more code here atm DBG_ASSERT(*((ULONG *)ptr)==DATABASE_VERSION); ptr += sizeof(ULONG); size -= sizeof(ULONG); /** * NOTE that since memory pools are being used, I free nothing * when a error happens, hence there is no problem on this code */ while(!error) { TaskReg * entry; if(size < (LONG)sizeof(ULONG)) { error = TRUE; break; } if(*((ULONG *) ptr ) == DATABASE_EOFID) break; if((entry = Malloc(sizeof(TaskReg)))) { GetV( entry->TaskNameLength, UWORD ); if(error) break; if((entry->TaskName = Malloc(entry->TaskNameLength+1))) { GetX( entry->TaskName, entry->TaskNameLength); if(error) break; GetV( entry->allow, BYTE ); GetV( entry->remember, BYTE ); if(error) break; GetV( entry->RegTime.ds_Days, ULONG ); GetV( entry->RegTime.ds_Minute, ULONG ); GetV( entry->RegTime.ds_Tick, ULONG ); GetV( entry->ModTime.ds_Days, ULONG ); GetV( entry->ModTime.ds_Minute, ULONG ); GetV( entry->ModTime.ds_Tick, ULONG ); GetV( entry->accesses, ULONG ); GetV( entry->FileCRC, ULONG ); GetV( entry->CRCMods, UWORD ); GetV( entry->AlertFlags, UWORD ); GetV( entry->ServerPort, UWORD ); if(error) break; AddTail((struct List *) G->TaskRegList, (struct Node *) entry ); #if DATABASE_RESERVED ptr += DATABASE_RESERVED; size -= DATABASE_RESERVED; #endif } else error = IoErr(); } else error = IoErr(); } if( error != 0 ) { if(error == ERROR_NO_FREE_STORE) { OutOfMemory("loading database"); rc = FALSE; } else if((rc = CorruptDatabase())) { TaskReg * entry; entry = (TaskReg *)((struct List *)G->TaskRegList)->lh_Head; while(TRUE) { struct MinNode * succ; if(entry == NULL) break; if(!(succ = ((struct MinNode *)entry)->mln_Succ)) break; Remove((struct Node *)entry); Free(entry->TaskName); Free(entry); entry = (TaskReg *) succ; } NewList((struct List *) G->TaskRegList ); } } } if( file != NULL ) FreeVec( file ); RETURN(rc); return(rc); }
/////////////////////////////////////////////////////////////////////////////// // GetHangulCharCluster // // The return value is the number of chars eaten; clusterSize is the number of // chars generated. In the large majority of cases, they will be equal. // eastl_size_t Typesetter::GetHangulCharCluster(eastl_size_t i, eastl_size_t iCharEnd, Char* pCharCluster, eastl_size_t& clusterSize, int& clusterType) { EA_ASSERT((i < iCharEnd) && (iCharEnd <= mLineLayout.mCharArray.size())); bool bCompose = false; // To do: Make this configurable, perhaps part of LayoutSettings. eastl_size_t charCount = 0; if(!bCompose) // If we leave the characters as-is and don't convert Jamo to composed syllables... { clusterType = kHangulClusterTypeUnicode; pCharCluster[0] = mLineLayout.mCharArray[i]; clusterSize = 1; charCount = 1; } else { clusterSize = 0; clusterType = kHangulClusterTypeJamo; // This might change below. for(const Char* p = &mLineLayout.mCharArray[i], *pEnd = &mLineLayout.mCharArray[iCharEnd]; (p < pEnd) && (clusterSize < kMaxHangulCharClusterSize); ++p, ++charCount) { const Char c = *p; const bool bIsAnyHangul = (IsS(c) || IsJ(c) || IsTone(c)); if(clusterSize != 0) // If this is not the first char in the cluster... { const Char cPrev = p[-1]; // If the current character cannot combine with the previous character... // To consider: This should be a table lookup instead of a bunch of comparisons. if(!bIsAnyHangul || ( IsT(cPrev) && IsL(c)) || ( IsV(cPrev) && IsL(c)) || ( IsT(cPrev) && IsV(c)) || (!IsL(cPrev) && IsS(c)) || IsTone(cPrev)) { // We have a completed syllable. break; } } if(!bIsAnyHangul) // If not any kind of Hangul (syllable, jamo, tone)... { // We have non-Hangul Unicode (e.g. Latin or simply a space chars). pCharCluster[clusterSize++] = c; ++charCount; clusterType = kHangulClusterTypeUnicode; break; } else if(IsS(c)) // If the char is a Hangul syllable... { // We break the Hangul syllable down to its Jamo components. // We'll put them back together again in the next step of // the shaping pipeline. pCharCluster[clusterSize++] = GetL(c); pCharCluster[clusterSize++] = GetV(c); if(HasT(c)) pCharCluster[clusterSize++] = GetT(c); } else if((clusterSize == 0) && IsTone(c)) // If char is an initial Hangul tone... { // We have a standalone tone. pCharCluster[clusterSize++] = c; ++charCount; clusterType = kHangulClusterTypeTone; break; } else pCharCluster[clusterSize++] = c; // Else we are working with individual Jamo symbols... } } EA_ASSERT_MESSAGE(clusterType != kHangulClusterTypeNone, "Typesetter::GetHangulCharCluster: Unknown cluster type."); EA_ASSERT((charCount > 0) && (clusterSize > 0)); return charCount; }