/*
* 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));
}
