/* * 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() { // create mutexes irDetectorMutex = mutexCreate(); piSetpointMutex = mutexCreate(); //initialize IMEs imeInitializeAll(); }
THREAD_ROUTINE_RETURN SocketServer::HandleConnections(void* data) { SocketServer* server = (SocketServer*) data; std::vector<SocketServer::Connection*> clients; struct sockaddr_storage client_addr; socklen_t addr_size = sizeof(client_addr); MutexHandle lock; mutexCreate(&lock); int client_socket; while (!server->shutdown_) { if ((client_socket = accept(server->socket_, (struct sockaddr*)&client_addr, &addr_size)) > 0) { clients.push_back(new jsonrpc::SocketServer::Connection()); clients.back()->socket = client_socket; clients.back()->pserver = server; clients.back()->plock_server = &lock; CloseFinishedConnections(clients); if (clients.size() > server->poolSize_) { CloseOldestConnection(clients); } threadCreate(&clients.back()->thread, (ThreadStartRoutine)ConnectionHandler, clients.back()); } } CloseAllConnections(clients); mutexDestroy(&lock); return 0; }
Flywheel *flywheelInit(FlywheelSetup setup) { Flywheel *flywheel = malloc(sizeof(Flywheel)); flywheel->target = 0.0f; flywheel->measured = 0.0f; flywheel->measured = 0.0f; flywheel->derivative = 0.0f; flywheel->integral = 0.0f; flywheel->error = 0.0f; flywheel->action = 0.0f; flywheel->lastAction = 0.0f; flywheel->lastError = 0.0f; flywheel->firstCross = true; flywheel->reading = 0; flywheel->microTime = micros(); flywheel->timeChange = 0.0f; flywheel->pidKp = setup.pidKp; flywheel->pidKi = setup.pidKi; flywheel->pidKd = setup.pidKd; flywheel->tbhGain = setup.tbhGain; flywheel->tbhApprox = setup.tbhApprox; flywheel->bangBangValue = setup.bangBangValue; flywheel->gearing = setup.gearing; flywheel->encoderTicksPerRevolution = setup.encoderTicksPerRevolution; flywheel->smoothing = setup.smoothing; flywheel->ready = true; flywheel->delay = FLYWHEEL_READY_DELAY; flywheel->allowReadify = true; flywheel->controllerType = CONTROLLER_TYPE_PID; flywheel->targetMutex = mutexCreate(); flywheel->task = NULL; //flywheel->task = taskCreate(task, 1000000, flywheel, FLYWHEEL_READY_PRIORITY); // TODO: What stack size should be set? flywheel->encoder = encoderInit(setup.encoderPortTop, setup.encoderPortBottom, setup.encoderReverse); flywheel->motorChannels[0] = setup.motorChannels[0]; flywheel->motorChannels[1] = setup.motorChannels[1]; flywheel->motorChannels[2] = setup.motorChannels[2]; flywheel->motorChannels[3] = setup.motorChannels[3]; flywheel->motorReversed[0] = setup.motorReversed[0]; flywheel->motorReversed[1] = setup.motorReversed[1]; flywheel->motorReversed[2] = setup.motorReversed[2]; flywheel->motorReversed[3] = setup.motorReversed[3]; return flywheel; }
extern void addTimer() { Thread thread; threadCurrent(&thread); Timer timer = { thread, getTime() }; if (!timerStack.mutexInit) { mutexCreate(&(timerStack.mutex)); timerStack.mutexInit = 1; } mutexLock(&(timerStack.mutex)); timerStack.stack[timerStack.top++] = timer; mutexUnlock(&(timerStack.mutex)); }