int taskResponseTest(void)
{
struct sigaction newAction;
//init signal
newAction.sa_handler = Action; //set the new handler
sigemptyset(&newAction.sa_mask); //no other signals blocked
newAction.sa_flags = 0; //no special options
if(sigaction(SIGINT, &newAction, NULL) == -1)
printf("Could not install signal handler\n");
//init semaphore
semMutex = semBCreate(SEM_Q_PRIORITY, SEM_EMPTY);
//create task A and B
if((taskRTA = taskCreate("taskRTA", 100, NULL, STACK_SIZE, (FUNCPTR)TaskRTA, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == NULL)
printf("Could not create task A\n");
if((taskRTB = taskCreate("taskRTB", 99, NULL, STACK_SIZE, (FUNCPTR)TaskRTB, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == NULL )
printf("Could not create task B\n");
taskRTMaster = taskIdSelf();
//resume task B
taskResume(taskRTB);
//resume task A
taskResume(taskRTA);
printf("It is task response test.\n");
return 0;
}
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());*/
}
// RobotMode robotMode;
void initialize() {
//gyro = gyroInit(1, 0);
// robotMode = STANDARD_MODE;
analogCalibrate(3);
claw.status = HOLDING;
claw.holdTarget = clawGetPosition();
claw.autoOpenPos = 0;
claw.autoOpenTrigger -1;
taskCreate(clawTask, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);
taskCreate(armTask, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);
leftEncoder = encoderInit(LEFT_ENC_1,LEFT_ENC_2,1);
rightEncoder = encoderInit(RIGHT_ENC_1,RIGHT_ENC_2,0);
}
void flywheelRun(Flywheel *flywheel)
{
if (!flywheel->task)
{
flywheelReset(flywheel);
flywheel->task = taskCreate(task, TASK_DEFAULT_STACK_SIZE, flywheel, FLYWHEEL_ACTIVE_PRIORITY);
}
}
void pLoopDriveStraightAsync(int ticks, bool correctBackwards,
bool correctDir) {
drStTicks = ticks;
drStCorrectBack = correctBackwards;
drStCorrectDir = correctDir;
drStDone = false;
taskCreate(driveStraight, TASK_DEFAULT_STACK_SIZE, NULL,
TASK_PRIORITY_DEFAULT);
}
/*
* Runs user initialization code. This function will be started in its own task with the default
* priority and stack size once when the robot is starting up. It is possible that the VEXnet
* communication link may not be fully established at this time, so reading from the VEX
* Joystick may fail.
*
* This function should initialize most sensors (gyro, encoders, ultrasonics), LCDs, global
* variables, and IMEs.
*
* This function must exit relatively promptly, or the operatorControl() and autonomous() tasks
* will not start. An autonomous mode selection menu like the pre_auton() in other environments
* can be implemented in this task if desired.
*/
void initialize() {
autoFire = 0;
launchThreshold = 3000;
queueThreshold = 3000;
riceBotInitialize();
shooterL1 = initRicemotor(2,-1);
shooterL2 = initRicemotor(3,-1);
shooterR1 = initRicemotor(8,1);
shooterR2 = initRicemotor(9,1);
ramp= initRicemotor(5, -1);
net = initRicemotor(7, -1);
ANALaunch = initRicesensorAnalog(1, false);
ANAQueue = initRicesensorAnalog(2, false);
taskCreate(IOTask, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_HIGHEST);
taskCreate(shooterTask, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);
}
void initLCDLList()
{
//char *strg;
lcdInit(uart1);
lcdClear(uart1);
lcdSetBacklight(uart1, true);
LCDTask = taskCreate(LCDRun, TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);
// strg = strcenter("test", 16);
//printf("|%s| ",strg/*,strcenter("Test", 16)*/);
}
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
}
void operatorControl()
{
flywheelRun(flywheel);
stdinHandlerRun();
taskCreate(streamOutTask, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);
while (1)
{
delay(1000);
}
}
/*
* Runs user initialization code. This function will be started in its own task
* with the default
* priority and stack size once when the robot is starting up. It is possible
* that the VEXnet
* communication link may not be fully established at this time, so reading from
* the VEX
* Joystick may fail.
*
* This function should initialize most sensors (gyro, encoders, ultrasonics),
* LCDs, global
* variables, and IMEs.
*
* This function must exit relatively promptly, or the operatorControl() and
* autonomous() tasks
* will not start. An autonomous mode selection menu like the pre_auton() in
* other environments
* can be implemented in this task if desired.
*/
void initialize() {
print("[Init] Initializing the robot\n");
// Set up the LCD and start it
print("[Init] Setting up the LCD\n");
lcdInitMenu(1, 3, 1); // Min 1, max 3, home 1
lcdSetUpdater(implUpdateLCD);
lcdSetMenuBack(implMenuBack);
lcdSetMenuNext(implMenuNext);
lcdSetCycles(true);
// Set up our drive
print("[Init] Setting up drive motors\n");
lcdSetText(uart2, 1, "Init drive...");
driveInit(MOTOR_DRIVE_FL, MOTOR_DRIVE_BL, MOTOR_DRIVE_FR, MOTOR_DRIVE_BR);
driveSetReverse(MOTOR_DRIVE_FL_REV, MOTOR_DRIVE_BL_REV, MOTOR_DRIVE_FR_REV,
MOTOR_DRIVE_BR_REV);
// enableSlew(15); // Set slew rate to 15
// Set up our autonomous to these modes
print("[Init] Setting up autonomous modes\n");
lcdSetText(uart2, 1, "Init auton...");
autonInit(4); // 3 auton modes
// Set up our gyroscope
print("[Init] Setting gyroscope\n");
lcdSetText(uart2, 1, "Init gyro...");
// To tune: 196*((360*rotations)/gyroValue)
gyro = gyroInit(ANALOG_GYRO, 190); // default is 196, this is after tune
// Set up our encoders
print("[Init] Setting up encoders\n");
lcdSetText(uart2, 1, "Init Encs...");
initDriveEncoders();
// initPidControl();
// Sets communication port for JINX data and start task to parse incoming
// messages.
print("[Init] Setting up JINX\n");
lcdSetText(uart2, 1, "Init JINX...");
initJINX(stdout);
delay(100);
taskCreate(JINXRun, TASK_DEFAULT_STACK_SIZE, NULL, (TASK_PRIORITY_DEFAULT));
delay(100);
// Done init
print("[Init] Finished, starting LCD menu\n");
lcdSetText(uart2, 1, "Init menu...");
lcdStartMenu();
// TODO Remove
hc05Init(1, false);
}
/*
* Runs the user operator control code. This function will be started in its own task with the
* default priority and stack size whenever the robot is enabled via the Field Management System
* or the VEX Competition Switch in the operator control mode. If the robot is disabled or
* communications is lost, the operator control task will be stopped by the kernel. Re-enabling
* the robot will restart the task, not resume it from where it left off.
*
* If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will
* run the operator control task. Be warned that this will also occur if the VEX Cortex is
* tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
*
* Code running in this task can take almost any action, as the VEX Joystick is available and
* the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly
* recommended to give other tasks (including system tasks such as updating LCDs) time to run.
*
* This task should never exit; it should end with some kind of infinite loop, even if empty.
*/
void operatorControl() {
// start IR detection task
taskCreate(vTaskFilter, 2048, NULL, TASK_PRIORITY_DEFAULT);
// start PI controller
taskCreate(vTaskPIController, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);
//start polling the UltraSonic sensor
taskCreate(vTaskUltraSonic, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);
// show the detected IR levels
/*short leftIR_l, centerIR_l, rightIR_l;
while(1) {
if(mutexTake(irDetectorMutex, 10)) {
leftIR_l = leftIR;
centerIR_l = centerIR;
rightIR_l = rightIR;
mutexGive(irDetectorMutex);
}
printf("Left: %d Center: %d Right: %d \n", leftIR_l, centerIR_l, rightIR_l);
taskDelay(100);
}*/
// advance 1000, reverse 1000, every 2 seconds
int multiplier = 1;
while(1) {
// take PI Mutex
mutexTake(piSetpointMutex, -1);
leftMotor.position += 1000*multiplier;
rightMotor.position += 1000*multiplier;
mutexGive(piSetpointMutex);
multiplier = -1*multiplier;
taskDelay(2000);
}
}
/**
* 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;
}
void pLoopTurnPointAsync(int angle) {
turnAngle = angle;
turnDone = false;
taskCreate(turnPoint, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);
}
void runMogoAsync(int speed, int runTime) {
mogoSpeed = speed;
mogoTime = runTime;
mogoDone = false;
taskCreate(mogoAction, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);
}
void runIntakeAsync(int target) {
setIntakeTarget(target);
taskCreate(intakeAction, TASK_DEFAULT_STACK_SIZE, NULL,
TASK_PRIORITY_DEFAULT);
}
void runLiftAsync(int target, bool shouldOvershoot) {
setLiftTarget(target);
setShouldOvershootLift(shouldOvershoot);
liftDone = false;
taskCreate(liftAction, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);
}
void runNode(void *data) {
MoveNode *currNode = (MoveNode *)data;
MoveNode *currChild = currNode->child;
if (currNode->nodeId == NULL) {
return;
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
Sensor *sensor = &pairMap[currNode->nPairId].sensors[i];
// createSensor(sensor);
/*if (sensor->type == SHAFT_ENCODER) {
sensor->enc = encoderInit(sensor->port, sensor->port + 1, false);
}*/
startSensor(sensor);
sensor = NULL;
}
signed char *saveState = NULL;
printDebug("Started node.");
// printf("Started node %d.\n\r", currNode->nodeId);
// printf("Node's child: %d\n\r", currChild->nodeId);
int nextPoint = 0;
setMotorSpeeds(currNode, nextPoint);
nextPoint++;
while (nextPoint < currNode->numPoints) {
// printf("DEBUG: %d\n\r", nextPoint);
while (joystickGetDigital(1, 5, JOY_UP)) { // pause
if (saveState == NULL) {
saveState = malloc(pairMap[currNode->nPairId].numPorts * sizeof(*(saveState)));
if (saveState == NULL) {
return; // break completely
}
for (int i = 0; i < pairMap[currNode->nPairId].numPorts; i++) {
saveState[i] = motorGet(pairMap[currNode->nPairId].motorPorts[i]);
motorStop(pairMap[currNode->nPairId].motorPorts[i]);
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
pauseTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
printDebug("Paused!");
}
if (joystickGetDigital(1, 8, JOY_DOWN)) { // stop it entirely
printDebug("Stopping!");
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
resumeTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
free(saveState);
saveState = NULL;
return;
}
delay(20);
}
if (outOfMemory) {
return;
}
if (saveState != NULL) {
for (int i = 0; i < pairMap[currNode->nPairId].numPorts; i++) {
motorSet(pairMap[currNode->nPairId].motorPorts[i], saveState[i]);
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
resumeTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
free(saveState);
saveState = NULL;
}
if (reachedPoint(currNode, currNode->points[nextPoint].endSensorVal, currNode->points[nextPoint - 1].endSensorVal)) {
setMotorSpeeds(currNode, nextPoint);
nextPoint++;
}
if (currChild != NULL) {
// printf("DEBUG: %d %d %d\n\r", currChild->nodeId, nextPoint, currChild->startPoint);
if (nextPoint + 1 >= currChild->startPoint && needToStart(currNode, currChild)) {
void *param = (void *)currChild;
taskCreate(runNode, TASK_DEFAULT_STACK_SIZE / 2, param, TASK_PRIORITY_DEFAULT);
currChild = currChild->sibling;
}
}
delay(5);
}
printDebug("Finished node.");
// printf("Finished node %d.", currNode->nodeId);
if (findParent(currNode)->nodeId == rootNode->nodeId) {
while (inMotion()) {
delay(20);
}
if (currNode->sibling != NULL) {
delay(currNode->sibling->startVal[0]);
runNode(currNode->sibling);
} else {
printDebug("Done.");
delay(500);
}
}
}
void stdinHandlerRun()
{
taskCreate(stdinHandler, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);
}
void blisten(char uart, void (*_callback)(char *)) {
uFile = uart == 1 ? uart1 : uart2;
callback = _callback;
taskCreate(blistenTask, TASK_DEFAULT_STACK_SIZE, NULL,
TASK_PRIORITY_DEFAULT);
}
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;
}
