task main()
{
clearDebugStream();
writeDebugStreamLine("This is PIDTest\n");
memset(&desiredEncVals, 0, sizeof(desiredEncVals));
motorInit(&desiredEncVals);
semaphoreInitialize(PIDconstantSemaphore);
startTask(PID);
long loopStartTimeMs = nPgmTime;
semaphoreLock(PIDconstantSemaphore);
for (pid_kp=0; pid_kp<5; pid_kp++) {
for (pid_ki=0; pid_ki<0; pid_ki+=0.01) {
for (pid_kd=0; pid_kd<0; pid_kd++) {
writeDebugStream("%f, %f, %f, ", pid_kp, pid_ki, pid_kd);
if (bDoesTaskOwnSemaphore(PIDconstantSemaphore)) {
semaphoreUnlock(PIDconstantSemaphore);
}
while(motorGetEncoder((tMotor) MecMotor_FR) < 5000){
hogCPU();
motorUpdateState();
desiredMotorVals.power[MecMotor_FR] = 50;
releaseCPU();
}
semaphoreLock(PIDconstantSemaphore);
motor[MecMotor_FR] = 0; //can do this because we have lock on semaphore
writeDebugStream("Changing constants!\n");
wait1Msec(1000); //wait for motor to spin down
}
}
}
}
void CMrcpTaskProcessor::ProcessTasks()
{
Name("ProcessTasks");
MrcpTasks* l_task;
boost::mutex::scoped_lock semaphoreLock( m_semaphoreMutex);
std::string l_statusInfo = "Entering Processing Thread: " + m_idString;
CLogger::Instance()->Log( LOG_LEVEL_INFO, *this, l_statusInfo);
{ // scope for lock
boost::mutex::scoped_lock l_controlLock( m_controlMutex);
EventProcessorRunning(true);
}
while( EventProcessorRunning())
{
if ( m_queue.empty())
{
m_semaphore.wait(semaphoreLock);
}
{ // scope for lock
boost::mutex::scoped_lock l_queueLock( m_queueMutex);
if ( !m_queue.empty ())
{
l_task = m_queue.front();
m_queue.pop();
}
else
{
continue;
}
}
try
{
l_task->Execute();
}
catch(...)
{
std::string l_debugInfo = "Caught Exception! debug info: " + MrcpUtils::itos((int)l_task->m_signalObj);
CLogger::Instance()->Log( LOG_LEVEL_ERROR, *this, "ProcessTasks", l_debugInfo);
}
delete l_task;
}
std::string l_stat2Info = "Exiting Processing Thread: " + m_idString;
CLogger::Instance()->Log( LOG_LEVEL_INFO, *this, l_stat2Info);
}
