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