task main() {
initializeRobot();
bool programRunning = false;
while (true) {
wait1Msec(5);
if (programStarted() && !programRunning) { // I.E. program JUST started
wait1Msec(selection[DELAY_MENU]*1000); // Wait for selected delay
currentRoutine=selection[STARTPOS_MENU]; // Set routine
if (gyroEnabled()) StartTask(gyroTask); // Start gyro tracking
resetGyro(); // Set gyro to 0
resetDrive(); // Set encoders to 0
programRunning=true; // Set the running veriable so this doesn't execute again
}
if (programStarted()) {
if (DEBUG) nxtDisplayCenteredTextLine(4, "%i", currentRoutine);
//Constantly update arm movements
if (armState != 0) motor[BlockArm] = getTargetingPower(armState, nMotorEncoder[BlockArm], 0.04, 80);
if (currentInc < 100 && !done) { // make sure step never wraps
if (completed) {
//Start sequences
initRoutine(); currentInc++; // Increment step, make sure it doesn't ever wrap
}
runTargets();
}
} else {
processMenuInput();
}
}
}
struct AHIBase*
gw_initRoutine( struct AHIBase* device,
APTR seglist,
struct ExecBase* sysbase )
{
return initRoutine( device, seglist, sysbase );
}
struct AHIBase* ASMCALL
gw_initRoutine( REG( d0, struct AHIBase* device ),
REG( a0, APTR seglist ),
REG( a6, struct ExecBase* sysbase ) )
{
return initRoutine( device, seglist, sysbase );
}
void ExecuteOnce(KDThreadOnce *onceControl, void (*initRoutine)(void))
{
const auto lpInitOnce = reinterpret_cast<PINIT_ONCE>(onceControl);
const auto dwFlags = 0;
const auto lpContext = nullptr;
BOOL pending;
InitOnceBeginInitialize(lpInitOnce, dwFlags, &pending, lpContext);
if (pending)
{
initRoutine();
InitOnceComplete(lpInitOnce, dwFlags, lpContext);
}
}
//A helper routine used to load a specified module described by module description.
static BOOL LoadModule(__EXTERNAL_MOD_DESC* pModDesc)
{
CHAR FullPathName[64];
CHAR Msg[128];
HANDLE hFile = NULL;
BYTE* pStartAddr = (BYTE*)pModDesc->StartAddress;
BOOL bResult = FALSE;
DWORD dwReadSize = 0;
BOOL bInitResult = FALSE;
__MODULE_INIT initRoutine = (__MODULE_INIT)pModDesc->StartAddress; //Initialize routine.
strcpy(FullPathName,OS_ROOT_DIR); //Append the root directory before module's file name.
strcat(FullPathName,pModDesc->ModFileName);
ToCapital(FullPathName);
//Now try to open the module.
hFile = CreateFile(FullPathName,
FILE_ACCESS_READ,
0,
NULL);
if(NULL == hFile) //Can not open the file.
{
sprintf(Msg,"Can not open the module named %s.",FullPathName);
PrintLine(Msg);
goto __TERMINAL;
}
//Now try to read the module file into memory,each for 8K byte.
do{
if(!ReadFile(hFile,
8192,
pStartAddr,
&dwReadSize)) //Failed to read.
{
sprintf(Msg,"Can not read from module file %s.",FullPathName);
PrintLine(Msg);
goto __TERMINAL;
}
pStartAddr += 8192; //Move to next 8k block.
}while(dwReadSize == 8192);
//Try to initialize the module.
if(pModDesc->dwModAttribute & MOD_ATTR_BIN) //BIN file.
{
bInitResult = initRoutine();
if(bInitResult)
{
sprintf(Msg,"Initialize module %s at 0x%X successful.",FullPathName,pModDesc->StartAddress);
PrintLine(Msg);
}
else
{
sprintf(Msg,"Initialize module %s at 0x%X failed.",FullPathName,pModDesc->StartAddress);
PrintLine(Msg);
}
}
if(pModDesc->dwModAttribute & MOD_ATTR_EXE) //Will be implemented later.
{
}
if(pModDesc->dwModAttribute & MOD_ATTR_EXE) //Will be implemented later.
{
}
sprintf(Msg,"Load module %s at 0x%X successful.",FullPathName,pModDesc->StartAddress);
PrintLine(Msg);
bResult = TRUE;
__TERMINAL:
if(NULL != hFile) //Should close it.
{
CloseFile(hFile);
}
return bResult;
}
int
pthread_once(pthread_once_t* onceControl, void (*initRoutine)(void))
{
// Algorithm:
// The state goes through at most four states:
// STATE_UNINITIALIZED: The initial uninitialized state.
// STATE_INITIALIZING: Set by the first thread entering the function. It
// will call initRoutine.
// semaphore/STATE_SPINNING: Set by the second thread entering the function,
// when the first thread is still executing initRoutine. The normal case is
// that the thread manages to create a semaphore. This thread (and all
// following threads) will block on the semaphore until the first thread is
// done.
// STATE_INITIALIZED: Set by the first thread when it returns from
// initRoutine. All following threads will return right away.
int32 value = atomic_test_and_set((vint32*)&onceControl->state,
STATE_INITIALIZING, STATE_UNINITIALIZED);
if (value == STATE_INITIALIZED)
return 0;
if (value == STATE_UNINITIALIZED) {
// we're the first -- perform the initialization
initRoutine();
value = atomic_set((vint32*)&onceControl->state, STATE_INITIALIZED);
// If someone else is waiting, we need to delete the semaphore.
if (value >= 0)
delete_sem(value);
return 0;
}
if (value == STATE_INITIALIZING) {
// someone is initializing -- we need to create a semaphore we can wait
// on
sem_id semaphore = create_sem(0, "pthread once");
if (semaphore >= 0) {
// successfully created -- set it
value = atomic_test_and_set((vint32*)&onceControl->state,
semaphore, STATE_INITIALIZING);
if (value == STATE_INITIALIZING)
value = semaphore;
else
delete_sem(semaphore);
} else {
// Failed to create the semaphore. Can only happen when the system
// runs out of semaphores, but we can still handle the situation
// gracefully by spinning.
value = atomic_test_and_set((vint32*)&onceControl->state,
STATE_SPINNING, STATE_INITIALIZING);
if (value == STATE_INITIALIZING)
value = STATE_SPINNING;
}
}
if (value >= 0) {
// wait on the semaphore
while (acquire_sem(value) == B_INTERRUPTED);
return 0;
} else if (value == STATE_SPINNING) {
// out of semaphores -- spin
while (atomic_get((vint32*)&onceControl->state) == STATE_SPINNING);
}
return 0;
}
