// **********************************************************************
// system kill task
//
int sysKillTask(int taskId)
{
Semaphore* sem = semaphoreList;
Semaphore** semLink = &semaphoreList;
// assert that you are not pulling the rug out from under yourself!
assert("sysKillTask Error" && tcb[taskId].name && superMode);
printf("\nKill Task %s", tcb[taskId].name);
// signal task terminated
semSignal(taskSems[taskId]);
// look for any semaphores created by this task
while(sem = *semLink)
{
if(sem->taskNum == taskId)
{
// semaphore found, delete from list, release memory
deleteSemaphore(semLink);
}
else
{
// move to next semaphore
semLink = (Semaphore**)&sem->semLink;
}
}
// ?? delete task from system queues
free (tcb[taskId].argv);
tcb[taskId].name = 0; // release tcb slot
return 0;
} // end sysKillTask
// **********************************************************************
// **********************************************************************
// Causes the system to shut down. Use this for critical errors
void powerDown(int code)
{
int i;
printf("\nPowerDown Code %d", code);
// release all system resources.
printf("\nRecovering Task Resources...");
// kill all tasks
for (i = MAX_TASKS-1; i >= 0; i--)
if(tcb[i].name) sysKillTask(i);
// delete all semaphores
while (semaphoreList)
deleteSemaphore(&semaphoreList);
// free ready queue/dc - SCOTT
free(readyQueue);
dcFreeMemory();
// ?? release any other system resources
// ?? deltaclock (project 3)
RESTORE_OS
return;
} // end powerDown
// **********************************************************************
// **********************************************************************
// create task
int createTask(char* name, // task name
int (*task)(int, char**), // task address
int priority, // task priority
int argc, // task argument count
char* argv[]) // task argument pointers
{
int tid;
// find an open tcb entry slot
for (tid = 0; tid < MAX_TASKS; tid++)
{
if (tcb[tid].name == 0)
{
char buf[8];
// create task semaphore
if (taskSems[tid]) deleteSemaphore(&taskSems[tid]);
sprintf(buf, "task%d", tid);
taskSems[tid] = createSemaphore(buf, 0, 0);
taskSems[tid]->taskNum = 0; // assign to shell
// copy task name
tcb[tid].name = (char*)malloc(strlen(name)+1);
strcpy(tcb[tid].name, name);
// set task address and other parameters
tcb[tid].task = task; // task address
tcb[tid].state = S_NEW; // NEW task state
tcb[tid].priority = priority; // task priority
tcb[tid].parent = curTask; // parent
tcb[tid].argc = argc; // argument count
// ?? malloc new argv parameters
tcb[tid].argv = malloc(argc*sizeof(char *));
int i;
for (i = 0; i < argc; i++)
tcb[tid].argv[i] = argv[i]; // argument pointers
tcb[tid].event = 0; // suspend semaphore
tcb[tid].RPT = 0; // root page table (project 5)
tcb[tid].cdir = CDIR; // inherit parent cDir (project 6)
// define task signals
createTaskSigHandlers(tid);
// Each task must have its own stack and stack pointer.
tcb[tid].stack = malloc(STACK_SIZE * sizeof(int));
// ?? may require inserting task into "ready" queue
if (tid) swapTask(); // do context switch (if not cli)
return tid; // return tcb index (curTask)
}
}
// tcb full!
return -1;
} // end createTask
// **********************************************************************
// system kill task
//
int sysKillTask(int taskId)
{
Semaphore* sem = semaphoreList;
Semaphore** semLink = &semaphoreList;
// assert that you are not pulling the rug out from under yourself!
assert("sysKillTask Error" && tcb[taskId].name && superMode);
printf("\nKill Task %s \n", tcb[taskId].name);
// signal task terminated
semSignal(taskSems[taskId]);
// look for any semaphores created by this task
while(sem = *semLink)
{
if(sem->taskNum == taskId)
{
// semaphore found, delete from list, release memory
deleteSemaphore(semLink);
}
else
{
// move to next semaphore
semLink = (Semaphore**)&sem->semLink;
}
}
// ?? delete task from system queues
dequeue(readyQueue, taskId); // SCOTT; others? flag
// SCOTT free mallocs
int i, numArgs;
numArgs = tcb[taskId].argc;
for (i = 0; i < tcb[taskId].argc; i++) {
free(tcb[taskId].argv[i]); // FLAG FIX
}
free(tcb[taskId].argv);
//println("argv memory free'd!");
// end SCOTT free mallocs
tcb[taskId].name = 0; // release tcb slot
return 0;
} // end sysKillTask
int MySemaphoreDestroy(MySemaphore sem)
{
printf("Inside Semaphore Destroy\n");
if(HasWaitingThreads(sem))
{
printf("\nCan't delete, waiting threads");
return -1;
}
if(deleteSemaphore(sem.semaphoreId))
{
printf("\nDeleted");
return 0;
}
else
{
printf("\nCan't delete, unknown reason");
return -1;
}
}
/**
* Free the resources for a timer event.
* All resources will be freed and the timer event will be removed from the
* queue if necessary.
*/
Notifier::~Notifier()
{
{
Synchronized sync(queueSemaphore);
DeleteFromQueue();
// Delete the static variables when the last one is going away
if (!(--refcount))
{
task->Stop();
delete task;
}
}
// Acquire the semaphore; this makes certain that the handler is
// not being executed by the interrupt manager.
takeSemaphore(m_handlerSemaphore);
// Delete while holding the semaphore so there can be no race.
deleteSemaphore(m_handlerSemaphore);
}
/**
* Free the resources for a timer event.
* All resources will be freed and the timer event will be removed from the
* queue if necessary.
*/
Notifier::~Notifier()
{
{
::std::unique_lock<ReentrantMutex> sync(queueSemaphore);
DeleteFromQueue();
// Delete the static variables when the last one is going away
if (!(--refcount))
{
int32_t status = 0;
cleanNotifier(m_notifier, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
// Acquire the semaphore; this makes certain that the handler is
// not being executed by the interrupt manager.
takeSemaphore(m_handlerSemaphore);
// Delete while holding the semaphore so there can be no race.
deleteSemaphore(m_handlerSemaphore);
}
int main(int argc, char const *argv[])
{
int semId = semget(IPC_PRIVATE,1,SHM_R|SHM_W);
printf("Id del semaforo es: %d\n", semId);
union semun arg;
int number;
printf("Escribe un número\n");
scanf("%d",&number);
arg.val = number;
int status = semctl(semId,0,SETVAL,arg);
if (status == -1){
printf("Error seting value\n");
deleteSemaphore(semId);
exit(-1);
}
return 0;
}
// **********************************************************************
// **********************************************************************
// create task
int createTask(char* name, // task name
int (*task)(int, char**), // task address
int priority, // task priority
int argc, // task argument count
char* argv[]) // task argument pointers
{
int tid;
// find an open tcb entry slot
for (tid = 0; tid < MAX_TASKS; tid++)
{
if (tcb[tid].name == 0)
{
char buf[8];
// create task semaphore
if (taskSems[tid]) deleteSemaphore(&taskSems[tid]);
sprintf(buf, "task%d", tid);
taskSems[tid] = createSemaphore(buf, 0, 0);
taskSems[tid]->taskNum = 0; // assign to shell
// copy task name
tcb[tid].name = (char*)malloc(strlen(name)+1);
strcpy(tcb[tid].name, name);
// set task address and other parameters
tcb[tid].taskTime = 0; // SCOTT - p5
tcb[tid].task = task; // task address
tcb[tid].state = S_NEW; // NEW task state
tcb[tid].priority = priority; // task priority
tcb[tid].parent = curTask; // parent
tcb[tid].argc = argc; // argument count
// SCOTT malloc new argv parameters
char** mallocdArgv = malloc(sizeof(char*) * MAX_ARGS);
int i;
for (i = 0; i < argc; i++) {
mallocdArgv[i] = malloc(sizeof(char) * MAX_ARG_LENGTH); // max arg length = 50
strcpy(mallocdArgv[i], argv[i]);
}
tcb[tid].argv = mallocdArgv; // argument pointers // change this line
// end malloc SCOTT
tcb[tid].event = 0; // suspend semaphore
// SCOTT - set RPT
//tcb[tid].RPT = 0; // root page table (project 5) 4??
tcb[tid].RPT = LC3_RPT + ((tid) ? ((tid - 1) << 6) : 0); // from slide 35
tcb[tid].cdir = CDIR; // inherit parent cDir (project 6)
// define task signals
createTaskSigHandlers(tid);
// Each task must have its own stack and stack pointer.
tcb[tid].stack = malloc(STACK_SIZE * sizeof(int));
// ?? may require inserting task into "ready" queue
enqueue(readyQueue, tid, tcb[tid].priority); // SCOTT
if (tid) swapTask(); // do context switch (if not cli)
return tid; // return tcb index (curTask)
}
}
// tcb full!
return -1;
} // end createTask
// **********************************************************************
// **********************************************************************
// create task
int createTask(char* name, // task name
int (*task)(int, char**), // task address
int priority, // task priority
int argc, // task argument count
char* argv[]) // task argument pointers
{
int tid;
// find an open tcb entry slot
for (tid = 0; tid < MAX_TASKS; tid++)
{
if (tcb[tid].name == 0)
{
int i = 0;
char** newArgv = malloc(argc * sizeof(char*));
for(i = 0; i < argc; i++)
{
newArgv[i] = malloc(sizeof(char) * (strlen(argv[i]) + 1));
strcpy(newArgv[i], argv[i]);
}
char buf[8];
// create task semaphore
if (taskSems[tid]) deleteSemaphore(&taskSems[tid]);
sprintf(buf, "task%d", tid);
taskSems[tid] = createSemaphore(buf, 0, 0);
taskSems[tid]->taskNum = 0; // assign to shell
// copy task name
tcb[tid].name = (char*)malloc(strlen(name)+1);
strcpy(tcb[tid].name, name);
// set task address and other parameters
tcb[tid].task = task; // task address
tcb[tid].state = S_NEW; // NEW task state
tcb[tid].priority = priority; // task priority
tcb[tid].time = 0; // time
tcb[tid].parent = curTask; // parent
//printf("\nCurTask: %d, Name: %s", curTask, tcb[curTask].name);
//printf("\nNewTask: %d, Name: %s", tid, tcb[tid].name);
tcb[tid].argc = argc; // argument count
// ?? malloc new argv parameters
tcb[tid].argv = newArgv; // argument pointers
tcb[tid].event = 0; // suspend semaphore
tcb[tid].RPT = 0x2440;//(tid - 1) * 64 + 0x2400; // root page table (project 5)
tcb[tid].cdir = CDIR; // inherit parent cDir (project 6)
// signals
tcb[tid].signal = 0;
if (tid)
{
// inherit parent signal handlers
tcb[tid].sigContHandler = tcb[curTask].sigContHandler; // task mySIGCONT handler
tcb[tid].sigIntHandler = tcb[curTask].sigIntHandler; // mySIGINT handler
tcb[tid].sigTermHandler = tcb[curTask].sigTermHandler; // task mySIGTERM handler
tcb[tid].sigTstpHandler = tcb[curTask].sigTstpHandler; // task mySIGTSTP handler
}
else
{
// otherwise use defaults
tcb[tid].sigContHandler = defaultSigContHandler; // task mySIGINT handler
tcb[tid].sigIntHandler = defaultSigIntHandler; // task mySIGINT handler
tcb[tid].sigTermHandler = defaultSigTermHandler; // task mySIGINT handler
tcb[tid].sigTstpHandler = defaultSigTstpHandler; // task mySIGINT handler
}
// Each task must have its own stack and stack pointer.
tcb[tid].stack = malloc(STACK_SIZE * sizeof(int));
// ?? may require inserting task into "ready" queue
enqueue(tid, &READY_QUEUE);
if (tid) swapTask(); // do context switch (if not cli)
return tid; // return tcb index (curTask)
}
}
// tcb full!
return -1;
} // end createTask
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
static bool firstTime = true;
initSemaphore(&stepSEM);
attachSEM(&stepSEM, STEP_SEM);
initSharedMemory(&stepSHM, sizeof(episode_steps));
attachSHM(&stepSHM, STEP_SHM, &stepSEM);
readSHMemory(&stepSHM, &episodeStep, &stepSEM);
int numStates = (int) *mxGetPr(prhs[0]);
int numSteps = episodeStep.numTransmittedSteps;
fprintf(stderr, "NumSteps: %d\n",numSteps);
plhs[0] = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
plhs[1] = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
plhs[2] = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
plhs[3] = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
plhs[4] = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
plhs[5] = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
plhs[6] = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
plhs[7] = mxCreateDoubleMatrix(7, numSteps, mxREAL);
plhs[8] = mxCreateDoubleMatrix(numStates, numSteps, mxREAL);
plhs[9] = mxCreateDoubleMatrix(1, numSteps, mxREAL);
plhs[10] = mxCreateDoubleMatrix(1, numSteps, mxREAL);
plhs[10] = mxCreateDoubleMatrix(1, numSteps, mxREAL);
double *joints = mxGetPr(plhs[0]);
double *jointsVel = mxGetPr(plhs[1]);
double *jointsAcc = mxGetPr(plhs[2]);
double *jointsDes = mxGetPr(plhs[3]);
double *jointsVelDes = mxGetPr(plhs[4]);
double *jointsAccDes = mxGetPr(plhs[5]);
double *torque = mxGetPr(plhs[6]);
double *cart = mxGetPr(plhs[7]);
double *state = mxGetPr(plhs[8]);
double *commandIdx = mxGetPr(plhs[9]);
double *stepInTrajectory = mxGetPr(plhs[10]);
double *doMotionIdx = mxGetPr(plhs[10]);
memcpy(joints, episodeStep.joints, sizeof(double) * numSteps * N_DOFS);
memcpy(jointsVel, episodeStep.jointsVel, sizeof(double) * numSteps * N_DOFS);
memcpy(jointsAcc, episodeStep.jointsAcc, sizeof(double) * numSteps * N_DOFS);
memcpy(jointsDes, episodeStep.jointsDes, sizeof(double) * numSteps * N_DOFS);
memcpy(jointsVelDes, episodeStep.jointsVelDes, sizeof(double) * numSteps * N_DOFS);
memcpy(jointsAccDes, episodeStep.jointsAccDes, sizeof(double) * numSteps * N_DOFS);
memcpy(torque, episodeStep.torque, sizeof(double) * numSteps * N_DOFS);
memcpy(cart, episodeStep.cart, sizeof(double) * numSteps * 7);
// printf("doMotionIdx :");
// for (int i = 0; i < numSteps; i ++)
// printf(" %d",episodeStep.doMotionIdx[i]);
// printf(" \n");
for (int i = 0; i < numSteps; i ++)
{
for (int j = 0; j < numStates; j ++)
{
state[i * numStates + j] = episodeStep.state[i][j];
// printf("%f ", episodeStep.state[i][j]);
}
// printf("\n");
}
for (int i = 0; i < numSteps; i ++)
{
commandIdx[i] = episodeStep.commandIdx[i];
doMotionIdx[i] = episodeStep.doMotionIdx[i];
}
for (int i = 0; i < numSteps; i ++)
{
stepInTrajectory[i] = episodeStep.stepInTrajectory[i];
}
deleteSemaphore(&stepSEM);
deleteSharedMemory(&stepSHM);
}
