int driveAndReadLight(float power, float distance, bool encodingCorrection) {
float photoValue = 3.3;
left.SetPower(power * LEFT_MODIFIER);
right.SetPower(power);
leftencoder.ResetCounts();
rightencoder.ResetCounts();
float start = TimeNow();
while (rightencoder.Counts() < distance * COUNTS_PER_INCH && TimeNow() - start < UNIVERSAL_TIMEOUT) {
if (rightencoder.Counts() % COUNTS_PER_CHECK == 0 && encodingCorrection) {
//Look at encoder counts and adjust right motor based on results
if (rightencoder.Counts() != 0) {
left.SetPower(power * rightencoder.Counts() / rightencoder.Counts());
}
}
LCD.Clear();
LCD.Write("The value of the left encoder is ");
LCD.WriteLine(rightencoder.Counts());
LCD.Write("The value of the right encoder is ");
LCD.WriteLine(rightencoder.Counts());
if (photosensor.Value() < photoValue) {
photoValue = photosensor.Value();
}
Sleep(50);
}
left.SetPower(0);
right.SetPower(0);
return photoValue < RED_BLUE_THRESHOLD ? RED : BLUE;
}
void turnUntilRPSHeading(int angle, float power, float timeoutSeconds, float turnModifier) {
int start = TimeNow();
while (abs(angle - wonka.Heading()) > RPS_TURNING_TOLERANCE && (TimeNow() - start) < timeoutSeconds) {
turnToRPSHeading(angle, power, CLOSEST, false, turnModifier);
Sleep(RPS_DELAY_TIME);
}
}
/* Rotates robot indefinitely.
*
* Direction of rotation is specified through the cw parameter.
*
* PARAM bot : struct robot*
* Points to struct with all the robot's peripherals
*
* PARAM cw : bool
* Whether the robot will turn clockwise. If false, the
* robot will turn counterclockwise
*
* RETURN void
* This method returns nothing
*/
void rot_tst(struct robot *bot, bool cw) {
// Variable declarations
double start_time = TimeNow();
double end_time;
// Begin rotation
if (cw) { // Rotate clockwise
(*bot->l_mot).SetPower(LM_PWR_RR);
(*bot->r_mot).SetPower(RM_PWR_RR);
} else { // Rotate counterclockwise
(*bot->l_mot).SetPower(LM_PWR_LR);
(*bot->r_mot).SetPower(RM_PWR_LR);
}
// Wait until the middle button is pressed
while (!(*bot->btns).MiddlePressed());
// Mark the time
end_time = TimeNow();
// Cease rotation
(*bot->l_mot).SetPower(0);
(*bot->r_mot).SetPower(0);
// Display elapsed time
LCD.Write("Time elapsed: ");
LCD.WriteLine(end_time - start_time);
// Give time to remove finger from button
Sleep(200);
}
/* Moves the robot backward indefinitely.
* Stops when middle button on button board is pressed.
*
* PARAM bot : struct robot*
* Points to struct with all the robot's peripherals
*
* RETURN void
* This method returns nothing
*/
void bck_tst(struct robot *bot) {
// Variable declarations
double start_time = TimeNow();
double end_time;
// Begin backward motion
(*bot->l_mot).SetPower(-1 * LM_PWR_FW);
(*bot->r_mot).SetPower(-1 * RM_PWR_FW);
// Wait until the middle button is pressed
while (!(*bot->btns).MiddlePressed());
// Mark the time
end_time = TimeNow();
// Cease backward motion
(*bot->l_mot).SetPower(0);
(*bot->r_mot).SetPower(0);
// Display elapsed time
LCD.Write("Time elapsed: ");
LCD.WriteLine(end_time - start_time);
// Give time to remove finger from button
Sleep(200);
}
VOID
BenchmarkThreadPool(
PLW_THREAD_POOL pPool,
PBENCHMARK_SETTINGS pSettings,
PULONG64 pullDuration,
PULONG64 pullBytesTransferred
)
{
PLW_TASK_GROUP pGroup = NULL;
PSOCKET* ppSockets1 = NULL;
PSOCKET* ppSockets2 = NULL;
size_t i = 0;
ULONG64 ullTotal = 0;
LONG64 llStart = 0;
LONG64 llEnd = 0;
ULONG64 ullTime = 0;
NTSTATUS status;
gpPool = pPool;
gpSettings = pSettings;
status = LwRtlCreateTaskGroup(gpPool, &pGroup);
ASSERT_SUCCESS(status);
status = LW_RTL_ALLOCATE_ARRAY_AUTO(&ppSockets1, gpSettings->ulPairs);
ASSERT_SUCCESS(status);
status = LW_RTL_ALLOCATE_ARRAY_AUTO(&ppSockets2, gpSettings->ulPairs);
ASSERT_SUCCESS(status);
for (i = 0; i < gpSettings->ulPairs; i++)
{
NTSTATUS status = CreateSocketPair(pGroup, &ppSockets1[i], &ppSockets2[i]);
ASSERT_SUCCESS(status);
}
status = TimeNow(&llStart);
ASSERT_SUCCESS(status);
LwRtlWakeTaskGroup(pGroup);
LwRtlWaitTaskGroup(pGroup);
status = TimeNow(&llEnd);
ASSERT_SUCCESS(status);
ullTime = (ULONG64) (llEnd - llStart);
LwRtlFreeTaskGroup(&pGroup);
for (i = 0; i < gpSettings->ulPairs; i++)
{
ullTotal += ppSockets1[i]->ullTotalTransferred;
}
*pullDuration = ullTime;
*pullBytesTransferred = ullTotal;
}
void TurnLeftForTime(float time)
{
rMotor.SetPercent(80);
lMotor.SetPercent(80);
float startTime = TimeNow();
float dTime = 0.0;
while( dTime < time)
{
dTime = TimeNow() - startTime;
}
rMotor.Stop();
lMotor.Stop();
}
void BackwardsForTime(float time)
{
rMotor.SetPercent(-1 * 80);
lMotor.SetPercent(80);
float startTime = TimeNow();
float dTime = 0.0;
while( dTime < time)
{
dTime = TimeNow() - startTime;
}
rMotor.Stop();
lMotor.Stop();
}
bool drivePastRPSCoordinate(float power, float coordinate, bool y, bool facingIncreasingDirection, float timeoutSeconds) {
float start = TimeNow();
while (y && (facingIncreasingDirection && wonka.Y() < coordinate || !facingIncreasingDirection && wonka.Y() > coordinate) ||
!y && (facingIncreasingDirection && wonka.X() < coordinate || !facingIncreasingDirection && wonka.X() > coordinate)
&& (TimeNow() - start) < timeoutSeconds) {
left.SetPower(power);
right.SetPower(power);
LCD.Write("RPS X: ");
LCD.WriteLine(wonka.X());
LCD.Write("RPS Y: ");
LCD.WriteLine(wonka.Y());
}
left.Stop();
right.Stop();
return (TimeNow() - start) < timeoutSeconds;
}
bool driveUntilSwitchPress(float power, int switchId, float timeoutDistance) {
leftencoder.ResetCounts();
rightencoder.ResetCounts();
left.SetPower(power * LEFT_MODIFIER);
right.SetPower(power);
float start = TimeNow();
while (((switchId == FRONT_SWITCH && frontSwitch.Value()) ||
(switchId == BACK_SWITCH && backSwitch.Value()) ||
(switchId == RIGHT_SWITCH && rightSwitch.Value() ||
(switchId == FORKLIFT_SWITCH && forkliftSwitch.Value()))) &&
rightencoder.Counts() < timeoutDistance * COUNTS_PER_INCH
&& TimeNow() - start < UNIVERSAL_TIMEOUT);
left.Stop();
right.Stop();
return rightencoder.Counts() < timeoutDistance * COUNTS_PER_INCH;
}
void MainWindow::CreateStatusbar()
{
shebei = new QLabel;
shebei->setText(tr("设备ID:"));
sbid = new QLabel;
sbid->setText(m_strDevID);
kjl = new QLabel;
kjl->setText(tr(" 快精灵"));
time = new QLabel;
QDateTime im = QDateTime::currentDateTime();
QString str = im.toString("yyyy-MM-dd hh:mm:ss");
time->setText(str);
QTimer* timTm = new QTimer;
connect(timTm,SIGNAL(timeout()),this,SLOT(TimeNow()));
timTm->start(1000);
jiaoshi = new ClickedLabel;
jiaoshi->setText(tr("<u>校时</u>"));
jiaoshi->setEnabled(false);
m_jiaozhun = new ClickedLabel;
m_jiaozhun->setText(tr("<u>屏幕校准</u>"));
connect(m_jiaozhun, SIGNAL(clicked()), this, SLOT(OnJiaoZhun()));
help = new ClickedLabel;
help->setText(tr("<u>帮助</u>"));
connect(help, SIGNAL(clicked()), this, SLOT(OnHelp()));
QStatusBar* statusBar = this->statusBar();
statusBar->addWidget(shebei,1);
statusBar->addWidget(sbid,2);
statusBar->addWidget(kjl,3);
statusBar->addWidget(time,4);
statusBar->addWidget(jiaoshi,5);
statusBar->addWidget(m_jiaozhun,6);
statusBar->addWidget(help,7);
}
static void HandleRPCFailure(long cid, long rcode, long op)
{
ConnVector[cid].Status = BROKEN;
if (rcode == RPC2_CONNBUSY)
{
connbusies++;
return;
}
if (op == 7 && rcode == RPC2_NAKED)
{
lostunbinds++;
assert(RPC2_Unbind(ConnVector[cid].ConnHandle) == RPC2_SUCCESS);
FLUSH();
return;
}
printf ("\n%s: call %ld on %#x to %s for %s failed (%s) at %s", MYNAME,
op, ConnVector[cid].ConnHandle,
ConnVector[cid].RemoteHost.Value.Name,
ConnVector[cid].NameBuf, RPC2_ErrorMsg(rcode), TimeNow());
DumpAndQuit(op);
}
static
NTSTATUS
WorkWait(
PLW_WORK_THREAD pThread
)
{
NTSTATUS status = STATUS_SUCCESS;
struct timespec ts = {0};
LONG64 llDeadline = 0;
int err = 0;
BOOLEAN bLastThread = FALSE;
PLW_WORK_THREADS pThreads = pThread->pThreads;
while (pThreads->ulQueued == 0)
{
if (pThreads->bShutdown)
{
status = STATUS_CANCELLED;
GOTO_ERROR_ON_STATUS(status);
}
if (pThread->pThreads->ulWorkThreadTimeout && !bLastThread)
{
status = TimeNow(&llDeadline);
GOTO_ERROR_ON_STATUS(status);
llDeadline += (LONG64) 1000000000ll * pThread->pThreads->ulWorkThreadTimeout;
ts.tv_sec = llDeadline / 1000000000ll;
ts.tv_nsec = llDeadline % 1000000000ll;
err = pthread_cond_timedwait(&pThread->pThreads->Event, &pThread->pThreads->Lock, &ts);
}
else
{
err = pthread_cond_wait(&pThread->pThreads->Event, &pThread->pThreads->Lock);
}
bLastThread = pThreads->ulWorkItemCount && pThreads->ulStarted == 1;
switch(err)
{
case ETIMEDOUT:
if (!bLastThread)
{
status = STATUS_TIMEOUT;
GOTO_ERROR_ON_STATUS(status);
}
break;
default:
status = LwErrnoToNtStatus(err);
GOTO_ERROR_ON_STATUS(status);
}
}
error:
return status;
}
void Game::PhaseChange(Phase::Base *next, Time transition_time)
{
if (!next)
return;
if (Transitioning())
EndTransition();
transition_ends = TimeNow() + transition_time;
next_phase = next;
}
void Sensor1ISR(void){
if(TimeSince(lastSensor1Trigger) > sensorTimeout){
lastSensor1Trigger = TimeNow();
Sensor1OutOn();
LogMsg(SENSOR, MESSAGE, "Sensor 1 Triggered");
TaskScheduleAdd(Sensor1OutOff, TASK_MEDIUM_PRIORITY, 10, 0);
}
ADCComparatorIntClear(ADC0_BASE, 0x0F);
}
void Sensor2ISR(void){
if(TimeSince(lastSensor2Trigger) > sensorTimeout){
lastSensor2Trigger = TimeNow();
Sensor2OutOn();
TaskScheduleAdd(Sensor2OutOff, TASK_MEDIUM_PRIORITY, 10, 0);
}
ADCComparatorIntClear(ADC0_BASE, 0x0F);
}
void drive(float power, float distance, bool encodingCorrection, bool debug, float leftModifier) {
if (debug) {
leftencoder.ResetCounts();
rightencoder.ResetCounts();
LCD.WriteLine("Press the switch to begin moving.");
while (stopSwitch.Value());
while (!stopSwitch.Value());
left.SetPower(power * LEFT_MODIFIER);
right.SetPower(power);
while (stopSwitch.Value());
while (!stopSwitch.Value());
left.Stop();
right.Stop();
LCD.Clear();
LCD.Write("The distance traveled was ");
LCD.WriteLine(rightencoder.Counts() * COUNTS_PER_INCH);
LCD.WriteLine("Press the switch to resume.");
while (stopSwitch.Value());
while (!stopSwitch.Value());
} else {
left.SetPower(power * LEFT_MODIFIER * leftModifier);
right.SetPower(power);
leftencoder.ResetCounts();
rightencoder.ResetCounts();
float start = TimeNow();
while (rightencoder.Counts() < distance * COUNTS_PER_INCH && TimeNow() - start < UNIVERSAL_TIMEOUT) {
if (rightencoder.Counts() % COUNTS_PER_CHECK == 0 && encodingCorrection) {
//Look at encoder counts and adjust right motor based on results
if (rightencoder.Counts() != 0) {
left.SetPower(power * rightencoder.Counts() / rightencoder.Counts());
}
}
LCD.Clear();
LCD.Write("The value of the left encoder is ");
LCD.WriteLine(rightencoder.Counts());
LCD.Write("The value of the right encoder is ");
LCD.WriteLine(rightencoder.Counts());
Sleep(50);
}
left.SetPower(0);
right.SetPower(0);
}
}
/*
* 打印日志
*
* @level 入参
* @src_file 入参
* @src_line 入参
* @fmt 入参
* @... 入参
*
* 返回：
* 0 成功
* -1 open错
* -2 配置错
*/
int Log(int level, char *src_file, int src_line, char *fmt, ...)
{
int fd;
char msg[1024 + 128] = {0};
char buf[1024 + 128] = {0};
char *pdate = DateNow(NULL);
char *ptime = TimeNow(NULL);
va_list args;
switch(gnLogType)
{
case 0:
fd = STDOUT_FILENO;
break;
case 1:
memset(gsLogFile, 0, sizeof(gsLogFile));
strcpy(gsLogFile, BuildFile("./log/logfile"));
fd = open(gsLogFile, O_RDWR | O_APPEND | O_CREAT, 0644);
if(fd == -1)
return -1;
break;
default:
return -2;
}
va_start(args, fmt);
vsprintf(msg, fmt, args);
va_end(args);
sprintf(buf + strlen(buf), "[PID=%5d]", getpid());
sprintf(buf + strlen(buf), "[%4s/", BufToStr(pdate, 4));
sprintf(buf + strlen(buf), "%2s", BufToStr(pdate + 4, 2));
sprintf(buf + strlen(buf), "/%2s]", BufToStr(pdate + 4, 2));
sprintf(buf + strlen(buf), "[%2s", BufToStr(ptime, 2));
sprintf(buf + strlen(buf), ":%2s", BufToStr(ptime + 2, 2));
sprintf(buf + strlen(buf), ":%2s]", BufToStr(ptime + 4, 2));
// sprintf(buf + strlen(buf), "[%15s: %04d]", src_file, src_line);
if(strlen(buf) == 0)
sprintf(buf, "%s\n", msg);
else
sprintf(buf + strlen(buf), " %s\n", msg);
write(fd, buf, strlen(buf));
if(fd != STDOUT_FILENO)
close(fd);
return 0;
}
void Pin::Run() {
//Back up, turn right
drive(FORWARD_POWER * -1, DISTANCE_1, false, false);
turnToRPSHeading(90, TURN_POWER, RIGHT, false, 1);
turnUntilRPSHeading(90, TURN_POWER);
//Drive to RPS coordinate representing skid alignment
drive(FORWARD_POWER, DISTANCE_1, false, false);
driveToRPSCoordinate(RPS_POWER, skidX, false, true);
Sleep(200);
//Adjust before pulling out pin
turnUntilRPSHeading(90, TURN_POWER);
//Drive backwards, move forklift down, drive forwards, move forklift up
drive (FORWARD_POWER * -1, DISTANCE_2, false, false);
forklift.SetDegree(PIN_DOWN_ANGLE);
float start = TimeNow();
while (TimeNow() - start < .5);
drive(FORWARD_POWER, DISTANCE_3, false, false);
forklift.SetDegree(PIN_ANGLE);
start = TimeNow();
while (TimeNow() - start < .5);
forklift.SetDegree(START_ANGLE);
//Drive back then forwards again
start = TimeNow();
while (TimeNow() - start < .5);
drive(PIN_POWER * -1, DISTANCE_3, false, false);
drive(PIN_POWER, DISTANCE_2, false, false);
}
void CGameConsole::Toggle(int Type)
{
if(m_ConsoleType != Type && (m_ConsoleState == CONSOLE_OPEN || m_ConsoleState == CONSOLE_OPENING))
{
// don't toggle console, just switch what console to use
}
else
{
if (m_ConsoleState == CONSOLE_CLOSED || m_ConsoleState == CONSOLE_OPEN)
{
m_StateChangeEnd = TimeNow()+m_StateChangeDuration;
}
else
{
float Progress = m_StateChangeEnd-TimeNow();
float ReversedProgress = m_StateChangeDuration-Progress;
m_StateChangeEnd = TimeNow()+ReversedProgress;
}
if (m_ConsoleState == CONSOLE_CLOSED || m_ConsoleState == CONSOLE_CLOSING)
{
m_OldMouseModes = Input()->GetMouseModes();
Input()->SetMouseModes(IInput::MOUSE_MODE_NO_MOUSE);
m_pClient->m_pMenus->UseMouseButtons(false);
m_ConsoleState = CONSOLE_OPENING;
// reset controls
m_pClient->m_pControls->OnReset();
}
else
{
Input()->SetMouseModes(m_OldMouseModes);
m_pClient->m_pMenus->UseMouseButtons(true);
m_pClient->OnRelease();
m_ConsoleState = CONSOLE_CLOSING;
}
}
m_ConsoleType = Type;
}
void SaveFramebuffer(VideoOutput& video, const Viewport& v)
{
#ifndef HAVE_GLES
const StreamInfo& si = video.Streams()[0];
if(video.Streams().size()==0 || (int)si.Width() != v.w || (int)si.Height() != v.h) {
video.Close();
return;
}
static basetime last_time = TimeNow();
const basetime time_now = TimeNow();
last_time = time_now;
static std::vector<unsigned char> img;
img.resize(v.w*v.h*4);
glReadBuffer(GL_BACK);
glPixelStorei(GL_PACK_ALIGNMENT, 1); // TODO: Avoid this?
glReadPixels(v.l, v.b, v.w, v.h, GL_RGB, GL_UNSIGNED_BYTE, &img[0] );
video.WriteStreams(&img[0]);
#endif // HAVE_GLES
}
void CGameConsole::Toggle(int Type)
{
if(m_ConsoleType != Type && (m_ConsoleState == CONSOLE_OPEN || m_ConsoleState == CONSOLE_OPENING))
{
// don't toggle console, just switch what console to use
}
else
{
if (m_ConsoleState == CONSOLE_CLOSED || m_ConsoleState == CONSOLE_OPEN)
{
m_StateChangeEnd = TimeNow()+m_StateChangeDuration;
}
else
{
float Progress = m_StateChangeEnd-TimeNow();
float ReversedProgress = m_StateChangeDuration-Progress;
m_StateChangeEnd = TimeNow()+ReversedProgress;
}
if (m_ConsoleState == CONSOLE_CLOSED || m_ConsoleState == CONSOLE_CLOSING)
{
/*Input()->MouseModeAbsolute();
m_pClient->m_pMenus->UseMouseButtons(false);*/
m_ConsoleState = CONSOLE_OPENING;
/*// reset controls
m_pClient->m_pControls->OnReset();*/
}
else
{
Input()->MouseModeRelative();
m_pClient->m_pMenus->UseMouseButtons(true);
m_pClient->OnRelease();
m_ConsoleState = CONSOLE_CLOSING;
}
}
m_ConsoleType = Type;
}
//METHOD 18
void Pause(double time)
{
SetPowerStop();
//rMotor.Stop();
//lMotor.Stop();
//Sleep(time);
double startTime = TimeNow();
double dTime = 0.0;
while( dTime < time )
{
logDataStuffs();
dTime = TimeNow() - startTime;
}
//rMotor.Stop();
//lMotor.Stop();
SetPowerStop();
}
/* Moves the robot forward indefinitely.
* Stops when middle button on button board is pressed.
*
* PARAM bot : struct robot*
* Points to struct with all the robot's peripherals
*
* RETURN void
* This method returns nothing
*/
void fwd_tst(struct robot *bot) {
// Variable declarations
double start_time = TimeNow();
double end_time;
struct log_data entry;
// Initialize log entry
entry.fname = "fwd_test";
entry.msg = "Moved forward for this many seconds";
// Begin forward motion
(*bot->l_mot).SetPower(LM_PWR_FW);
(*bot->r_mot).SetPower(RM_PWR_FW);
// Wait until the middle button is pressed
while (!(*bot->btns).MiddlePressed());
// Mark the time
end_time = TimeNow();
// Cease forward motion
(*bot->l_mot).SetPower(0);
(*bot->r_mot).SetPower(0);
// Display elapsed time
LCD.Write("Time elapsed: ");
LCD.WriteLine(end_time - start_time);
// Update log entry
entry.value = end_time - start_time;
// Give time to remove finger from button
Sleep(250);
// Log journal entry
bot->journal = log(bot->journal, &entry);
}
static void ListenerBody(void *arg)
{
long i;
RPC2_RequestFilter reqfilter;
RPC2_PacketBuffer *InBuff;
RPC2_NewConnectionBody *newconnbody;
RPC2_Handle newcid;
LWP_DispatchProcess(); /* initial courtesy to parent */
reqfilter.FromWhom = ONESUBSYS;
reqfilter.ConnOrSubsys.SubsysId = SUBSYS_SRV;
assert(reqfilter.ConnOrSubsys.SubsysId != -1);
reqfilter.OldOrNew = NEW;
InBuff = NULL;
while (1)
{
RanDelay(MaxListenPause);
if (InBuff != NULL) RPC2_FreeBuffer(&InBuff);
i = RPC2_GetRequest(&reqfilter, &newcid, &InBuff, NULL, GetPasswd, RPC2_XOR, NULL);
if (i != RPC2_SUCCESS)
{
printf("Listener error: ");
WhatHappened(i);
}
switch(InBuff->Header.Opcode)
{
case RPC2_NEWCONNECTION: /* new connection */
{
newconnbody = (RPC2_NewConnectionBody *)InBuff->Body;
if (VerboseFlag)
fprintf(stderr, "Newconn: %#x \"%s\" at %s",
newcid, (char*)&newconnbody->ClientIdent_SeqBody,
TimeNow());
RPC2_Enable(newcid); /* unfreeze the connection */
break;
}
default: /* unknown opcode */
assert(InBuff->Header.Opcode == RPC2_NEWCONNECTION);
break;
}
}
}
static void BulkErr(RPC2_Handle cid, SE_Descriptor *sed, int retcode, int op)
{
char *x;
printf ("\n%s: File transfer failed conn: %#x code: %s op: %d time: %s\n",
MYNAME, cid, RPC2_ErrorMsg(retcode), op, TimeNow());
if (sed->Value.SmartFTPD.TransmissionDirection == CLIENTTOSERVER)
x = "CLIENTTOSERVER";
else x = "SERVERTOCLIENT";
printf("\t\tFile: %s Direction: %s\n",
sed->Value.SmartFTPD.FileInfo.ByName.LocalFileName, x);
DumpAndQuit(op);
}
void DelayQueue::synchronize() {
// First, figure out how much time has elapsed since the last sync:
EventTime timeNow = TimeNow();
DelayInterval timeSinceLastSync = timeNow - fLastSyncTime;
fLastSyncTime = timeNow;
// Then, adjust the delay queue for any entries whose time is up:
DelayQueueEntry* curEntry = head();
while (timeSinceLastSync >= curEntry->fDeltaTimeRemaining) {
timeSinceLastSync -= curEntry->fDeltaTimeRemaining;
curEntry->fDeltaTimeRemaining = DELAY_ZERO;
curEntry = curEntry->fNext;
}
curEntry->fDeltaTimeRemaining -= timeSinceLastSync;
}
bool SharedMemoryVideo::GrabNext(unsigned char* image, bool wait)
{
// If a condition variable exists, try waiting on it.
if(_buffer_full) {
if (wait) {
_buffer_full->wait();
} else if (!_buffer_full->wait(TimeNow())) {
return false;
}
}
// Read the buffer.
_shared_memory->lock();
memcpy(image, _shared_memory->ptr(), _frame_size);
_shared_memory->unlock();
return true;
}
void DelayQueue::synchronize() {
// First, figure out how much time has elapsed since the last sync:
EventTime timeNow = TimeNow();
if (timeNow < fLastSyncTime) {
// The system clock has apparently gone back in time; reset our sync time and return:
fLastSyncTime = timeNow;
return;
}
DelayInterval timeSinceLastSync = timeNow - fLastSyncTime;
fLastSyncTime = timeNow;
// Then, adjust the delay queue for any entries whose time is up:
DelayQueueEntry* curEntry = head();
while (timeSinceLastSync >= curEntry->fDeltaTimeRemaining) {
timeSinceLastSync -= curEntry->fDeltaTimeRemaining;
curEntry->fDeltaTimeRemaining = DELAY_ZERO;
curEntry = curEntry->fNext;
}
curEntry->fDeltaTimeRemaining -= timeSinceLastSync;
}
void driveToRPSCoordinate(float power, float coordinate, bool y, bool facingIncreasingDirection, float degree) {
float start = TimeNow();
bool direction; //true if forward
while ((y && abs(coordinate - wonka.Y()) > RPS_DISTANCE_TOLERANCE && wonka.Y() != 0.0) || (!y && abs(coordinate - wonka.X()) > RPS_DISTANCE_TOLERANCE && wonka.X() != 0.0) && (TimeNow() - start) < LINEAR_TIME_LIMIT) {
if (y) {
direction = wonka.Y() < coordinate == facingIncreasingDirection;
} else {
direction = wonka.X() < coordinate == facingIncreasingDirection;
}
drive(FORWARD_POWER * (direction ? 1 : -1), RPS_DISTANCE_TOLERANCE / 2, false, false);
LCD.Write("RPS X: ");
LCD.WriteLine(wonka.X());
LCD.Write("RPS Y: ");
LCD.WriteLine(wonka.Y());
if (degree >= 0) {
turnToRPSHeading(degree, power, CLOSEST, false, 0.25);
}
Sleep(RPS_DELAY_TIME);
}
left.Stop();
right.Stop();
}
void turn(bool isRight, float power, int degrees, bool withSkid) {
if (!isRight) {
right.SetPower(power);
left.SetPower(-1 * power * LEFT_MODIFIER);
} else {
right.SetPower(-1 * power);
left.SetPower(power * LEFT_MODIFIER);
}
leftencoder.ResetCounts();
rightencoder.ResetCounts();
float start = TimeNow();
while (rightencoder.Counts() < degrees * (isRight ? countsPerDegreeRight : countsPerDegreeLeft) / (withSkid ? 1.0 : 1.0) && TimeNow() - start < UNIVERSAL_TIMEOUT) {
LCD.Clear();
LCD.Write("The value of the left encoder is ");
LCD.WriteLine(rightencoder.Counts());
LCD.Write("The value of the right encoder is ");
LCD.WriteLine(rightencoder.Counts());
Sleep(50);
}
left.SetPower(0);
right.SetPower(0);
}
