void OperatorControl()
{
//unsigned char c;
tid=taskCreate("Do Stuff Related To Work",100,0,0x1000,(FUNCPTR)MecanumThread,4,0,0,0,0,0,0,0,0,0);
taskActivate(tid);
lolcoder->Start();
lolcoder->Reset();
/*while (!leftStick->GetButton(Joystick::kTopButton))
{
bar++;
if (bar>=100000)
{
if (leftStick->GetButton(Joystick::kTriggerButton))
{
target+=10;
_DEBPRINT("Increasing target to: %d\n", target);
}
bar=0;
}
}*/
/*do
{
i=lolcoder->Get();
c=PCntrl(target,i);
j=c/255.0;
if (bar>=3000)
{
j=c/255.0;
_DEBPRINT("%f\n",j);
bar=0;
}
myRobot->Drive(j,0);
bar++;
} while(i<=target&&ds->IsEnabled());*/
//lolcoder->Stop();
//myRobot->Drive(0,0);
/*lolcoder->Reset();
do
{
bar++;
error=((float)target-(float)lolcoder->Get())*KP;
if (error>1)
error=1;
if (error<-1)
error=-1;
if (bar>=100000)
{
_DEBPRINT("%f\n",error);
bar=0;
if (leftStick->GetButton(Joystick::kTriggerButton))
{
target+=10;
_DEBPRINT("Increasing target to: %d\n", target);
}
}
speed=(char)((error*128)+128);
motor->SetRaw(speed);
} while (ds->IsEnabled());*/
}
int run_test_process(ContextAttachCallBack * done, void * data) {
#if defined(WIN32)
char fnm[FILE_PATH_SIZE];
char cmd[FILE_PATH_SIZE];
int res = 0;
STARTUPINFO si;
PROCESS_INFORMATION prs;
ContextAttachArgs * args;
memset(&si, 0, sizeof(si));
memset(&prs, 0, sizeof(prs));
memset(fnm, 0, sizeof(fnm));
if (GetModuleFileName(NULL, fnm, sizeof(fnm)) == 0) {
set_win32_errno(GetLastError());
return -1;
}
si.cb = sizeof(si);
strcpy(cmd, "agent.exe -t");
if (CreateProcess(fnm, cmd, NULL, NULL,
FALSE, CREATE_SUSPENDED | CREATE_DEFAULT_ERROR_MODE | CREATE_NO_WINDOW,
NULL, NULL, &si, &prs) == 0) {
set_win32_errno(GetLastError());
return -1;
}
args = (ContextAttachArgs *)loc_alloc(sizeof(ContextAttachArgs));
args->done = done;
args->data = data;
args->thread = prs.hThread;
args->process = prs.hProcess;
res = context_attach(prs.dwProcessId, done_context_attach, args, 0);
if (res != 0) loc_free(args);
return res;
#elif defined(_WRS_KERNEL)
int tid = taskCreate("tTcf", 100, 0, 0x4000, (FUNCPTR)test_proc, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
if (tid == 0) return -1;
taskStop(tid);
taskActivate(tid);
assert(taskIsStopped(tid));
return context_attach(tid, done, data, 0);
#else
/* Create child process to debug */
int pid = fork();
if (pid < 0) return -1;
if (pid == 0) {
int fd;
if (context_attach_self() < 0) exit(1);
fd = sysconf(_SC_OPEN_MAX);
while (fd-- > 2) close(fd);
if (tkill(getpid(), SIGSTOP) < 0) exit(1);
test_proc();
exit(0);
}
return context_attach(pid, done, data, CONTEXT_ATTACH_SELF);
#endif
}
static int __wind_task_activate(struct task_struct *curr, struct pt_regs *regs)
{
WIND_TCB *pTcb = (WIND_TCB *)xnregistry_fetch(__xn_reg_arg1(regs));
if (!pTcb)
return S_objLib_OBJ_ID_ERROR;
if (taskActivate((TASK_ID) pTcb) == ERROR)
return wind_errnoget();
return 0;
}
static int __wind_task_activate(struct pt_regs *regs)
{
WIND_TCB *pTcb = __wind_lookup_task(__xn_reg_arg1(regs));
if (!pTcb)
return S_objLib_OBJ_ID_ERROR;
if (taskActivate((TASK_ID) pTcb) == ERROR)
return wind_errnoget();
return 0;
}
/**
* Create a new TaskExecutor and native thread.
*/
TaskExecutor* createTaskExecutor(char* name, int priority, int stacksize) {
int taskID;
TaskExecutor* te = (TaskExecutor*)malloc(sizeof(TaskExecutor));
if (te == NULL) {
return NULL;
}
if (stacksize == 0) {
stacksize = DEFAULT_STACK_SIZE;
}
if (stacksize < MIN_STACK_SIZE) {
stacksize = MIN_STACK_SIZE;
}
te->runQ = NULL;
te->monitor = SimpleMonitorCreate();
if (te->monitor == NULL) {
te->status = EVENT_REQUEST_STATUS_ERROR;
te->te_errno = errno;
if (DEBUG_EVENTS_LEVEL) { fprintf(stderr, "In createTaskExecutor, error in SimpleMonitorCreate: %d%s\n", errno); }
return te;
}
te->status = TASK_EXECUTOR_STATUS_STARTING;
taskID = taskCreate(name,
taskPriorityMap(priority),
VX_FP_TASK, // options
stacksize, // stack size
(void*)teLoopingHandler, // function to start
(int)te, 0, 0, 0, 0, 0, 0, 0, 0, 0);
if (taskID == ERROR) {
te->status = EVENT_REQUEST_STATUS_ERROR;
te->te_errno = errno;
if (DEBUG_EVENTS_LEVEL) { fprintf(stderr, "In createTaskExecutor, error in taskCreate: %d%s\n", errno); }
} else {
if (DEBUG_EVENTS_LEVEL) { fprintf(stderr, "In createTaskExecutor, about to start new thread %s\n", name); }
if (taskActivate(taskID) == ERROR) {
te->status = EVENT_REQUEST_STATUS_ERROR;
te->te_errno = errno;
if (DEBUG_EVENTS_LEVEL) { fprintf(stderr, "In createTaskExecutor, error in taskActivate: %d%s\n", errno); }
}
}
/* TODO: Record TaskExecutor on global list of all TaskExecutors */
return te;
}
int taskSpawn(
const char *name,
unsigned priority,
int options,
unsigned stackSize,
FUNCPTR func,
ARG arg0,
ARG arg1,
ARG arg2,
ARG arg3,
ARG arg4,
ARG arg5,
ARG arg6,
ARG arg7,
ARG arg8,
ARG arg9
)
{
int taskId;
/* Create task */
taskId = taskCreat(
name,
priority,
options,
stackSize,
func,
arg0,
arg1,
arg2,
arg3,
arg4,
arg5,
arg6,
arg7,
arg8,
arg9
);
if (taskId != 0)
{
/* Then start it */
taskActivate(taskId);
}
return taskId;
}
STATUS taskRestart(
int taskId
)
{
TCB_ID tcbId;
char *name, *rename;
int len;
unsigned priority;
int options;
char *pStackBase;
unsigned stackSize;
FUNCPTR entry;
ARG args[MAX_TASK_ARGS];
STATUS status;
if (INT_RESTRICT() != OK)
{
errnoSet(S_intLib_NOT_ISR_CALLABLE);
return ERROR;
}
/* If self restart */
if ( (taskId == 0) || (taskId == (int) taskIdCurrent) )
{
/* Task must be unsafe */
while (taskIdCurrent->safeCount > 0)
taskSafe();
/* Spawn a task that will restart this task */
taskSpawn(restartTaskName, restartTaskPriority, restartTaskOptions,
restartTaskStackSize, taskRestart, (ARG) taskIdCurrent,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0);
/* Wait for restart */
while (1)
taskSuspend(0);
} /* End if self restart */
/* Get task context */
tcbId = taskTcb(taskId);
if (tcbId == NULL)
return ERROR;
/* TASK_ID_VERIFY() already done by taskTcb() */
/* Copy task data */
priority = tcbId->priority;
options = tcbId->options;
entry = tcbId->entry;
pStackBase = tcbId->pStackBase;
stackSize = (tcbId->pStackEnd - tcbId->pStackBase) * _STACK_DIR;
taskArgGet(tcbId, pStackBase, args);
/* Copy name if needed */
name = tcbId->name;
rename = NULL;
if (name != NULL) {
len = strlen(name) + 1;
rename = malloc(len);
if (rename != NULL)
strcpy(rename, name);
name = rename;
}
/* Prevent deletion */
taskSafe();
if (taskTerminate((int) tcbId) != OK)
{
taskUnsafe();
/* errno set by taskTerminate() */
return ERROR;
}
/* Initialize task with same data */
status = taskInit(tcbId, name, priority, options, pStackBase,
stackSize, entry, args[0], args[1], args[2],
args[3], args[4], args[5], args[6], args[7],
args[8], args[9]);
if (status != OK)
{
/* errno set by taskInit() */
return ERROR;
}
/* And start it */
status = taskActivate((int) tcbId);
if (status != OK)
{
/* errno set by taskActivate() */
return ERROR;
}
/* Make me mortal */
taskUnsafe();
/* Free rename buffer if needed */
if (rename != NULL)
free(rename);
return OK;
}
static int start_process(Channel * c, char ** envp, char * dir, char * exe, char ** args, int attach,
int * pid, int * selfattach, ChildProcess ** prs) {
int err = 0;
char * ptr;
SYM_TYPE type;
if (symFindByName(sysSymTbl, exe, &ptr, &type) != OK) {
err = errno;
if (err == S_symLib_SYMBOL_NOT_FOUND) err = ERR_SYM_NOT_FOUND;
assert(err != 0);
}
else {
int i;
int pipes[2][2];
/* TODO: arguments, environment */
*pid = taskCreate("tTcf", 100, 0, 0x4000, (FUNCPTR)ptr, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
for (i = 0; i < 2; i++) {
char pnm[32];
char pnm_m[32];
char pnm_s[32];
snprintf(pnm, sizeof(pnm), "/pty/tcf-%0*lx-%d", sizeof(*pid) * 2, *pid, i);
snprintf(pnm_m, sizeof(pnm_m), "%sM", pnm);
snprintf(pnm_s, sizeof(pnm_m), "%sS", pnm);
if (ptyDevCreate(pnm, PIPE_SIZE, PIPE_SIZE) == ERROR) {
err = errno;
break;
}
pipes[i][0] = open(pnm_m, O_RDWR, 0);
pipes[i][1] = open(pnm_s, O_RDWR, 0);
if (pipes[i][0] < 0 || pipes[i][1] < 0) {
err = errno;
break;
}
}
if (err) {
taskDelete(*pid);
*pid = 0;
}
else {
semTake(prs_list_lock, WAIT_FOREVER);
ioTaskStdSet(*pid, 0, pipes[0][1]);
ioTaskStdSet(*pid, 1, pipes[0][1]);
ioTaskStdSet(*pid, 2, pipes[1][1]);
*prs = loc_alloc_zero(sizeof(ChildProcess));
(*prs)->inp = pipes[0][0];
(*prs)->out = pipes[0][0];
(*prs)->err = pipes[1][0];
(*prs)->pid = *pid;
(*prs)->bcg = c->bcg;
list_add_first(&(*prs)->link, &prs_list);
if (attach) {
taskStop(*pid);
taskActivate(*pid);
assert(taskIsStopped(*pid));
}
else {
taskActivate(*pid);
}
semGive(prs_list_lock);
}
}
if (!err) return 0;
errno = err;
return -1;
}