int track(int trans_vel, int rot_vel, int clicks)
{
long time_out = mseconds() + 5000L; /* Time out after 5 seconds */
int l_tot_clks = 0; /* Total clicks */
int r_tot_clks = 0;
int ave_clks = 0;
get_left_clicks(); /* Reset clicks */
get_right_clicks(); /* Reset clicks */
driveb(trans_vel, rot_vel); /* Turn motors on */
while(1)
{
l_tot_clks = l_tot_clks + get_left_clicks(); /* Count up the clicks */
r_tot_clks = r_tot_clks + get_right_clicks();
ave_clks = min(l_tot_clks, r_tot_clks); /* Trigger on ave_clicks */
if((ave_clks >= clicks) || (mseconds() > time_out))
{
driveb((- trans_vel), (- rot_vel)); /* Try to kill motion */
sleep(stop_time);
stop();
return (clicks - ave_clks);
}
}
}
void msleep(long msec)
{
long end_time= mseconds() + msec;
while (1) {
/* if the following test doesn't execute at least once a second,
msleep may not halt */
long done= mseconds()-end_time;
if (done >= 0L && done <= 1000L) break;
}
}
float seconds()
{
return ((float) mseconds()) / 1000.;
}
int timer_done(long timer)
{
return timer < mseconds();
}
long timer_create_seconds(float timeout)
{
return mseconds() + (long) (timeout * 1000.);
}
long timer_create_mseconds(long timeout)
{
return mseconds() + timeout;
}
void ReadEncoders()
{
int i;
cLLast = cL;
cRLast = cR;
cL = read_encoder(0);
cR = read_encoder(1);
/* Stalled motors */
if (cL == cLLast && cR == cRLast)
{
if (!fStalling)
msStall = mseconds() + 250L;
fStalling = 1;
}
else
fStalling = 0;
if (fStalling && mseconds() >= msStall)
{
printf("Stall!\n");
beep();beep();
fStalled = 1;
}
/* Read bump sensors and do block detection */
fBlocked = 0;
for (i = 0; i < 4; i++)
{
rgfBump[i] = digital(i+12);
if (rgfBump[i] && sgn(rgpwr[rgibumpMotor[i]]) == rgibumpSgn[i])
{
fBlocked = 1;
rgfBlock[i] = 1;
}
else
rgfBlock[i] = 0;
}
if (digital(10))
fBall = 1;
}
/* Measure ticks per second */
void CalibrateSpeed()
{
long msStart;
long msEnd;
int cForward;
int cBack;
int cTicks = 500;
int i;
int fServo;
for (fServo = 0; fServo < 2; fServo++)
{
if (fServo)
{
enable_servos();
Gate(0);
printf("Servo: ");
}
else
printf("No Servo: ");
for (i = 0; i < 2; i++)
{
msStart = mseconds();
Move(cTicks,cTicks);
msEnd = mseconds();
rgSpeed[i] = MultDiv(100, 1000, (int) (msEnd - msStart));
printf("%d ", rgSpeed[i]);
cTicks = -cTicks;
sleep(1.0);
}
if (fServo)
disable_servos();
printf("\n");
StartPress();
}
}
void sample_sound()
{ int s_level = 0; /* Instantaneous sound level */
long go_low_time = 0L;
long current_time = 0L;
while (1)
{ while(listen_p)
{ current_time = mseconds();
s_level = abs(analog(microphone) - 128); /* Abs difference from 128 */
if (s_level > s_level_max)
s_level_max = s_level;
if ( s_level > sound_delta)
go_low_time = current_time + hold_time; /* Compute new go low time */
if (current_time > go_low_time)
loud_p = 0;
else
{ if (loud_p == 0) /* Was 0, now 1 => add to transition count */
sound_count++;
loud_p = 1; }
}}}
static long
consread(Chan *c, void *buf, long n, vlong offset)
{
int l;
Osenv *o;
int ch, eol, i;
char *p, tmp[128];
char *cbuf = buf;
if(n <= 0)
return n;
o = up->env;
switch((ulong)c->qid.path){
case Qdir:
return devdirread(c, buf, n, consdir, nelem(consdir), devgen);
case Qsysctl:
return readstr(offset, buf, n, VERSION);
case Qcons:
case Qkeyboard:
qlock(&kbd);
if(waserror()) {
qunlock(&kbd);
nexterror();
}
if(kbd.raw || kbd.kbdr) {
if(qcanread(lineq))
n = qread(lineq, buf, n);
else {
/* read as much as possible */
do {
i = qread(kbdq, cbuf, n);
cbuf += i;
n -= i;
} while(n>0 && qcanread(kbdq));
n = cbuf - (char*)buf;
}
} else {
while(!qcanread(lineq)) {
qread(kbdq, &kbd.line[kbd.x], 1);
ch = kbd.line[kbd.x];
eol = 0;
switch(ch){
case '\b':
if(kbd.x)
kbd.x--;
break;
case 0x15:
kbd.x = 0;
break;
case '\n':
case 0x04:
eol = 1;
default:
kbd.line[kbd.x++] = ch;
break;
}
if(kbd.x == sizeof(kbd.line) || eol) {
if(ch == 0x04)
kbd.x--;
qwrite(lineq, kbd.line, kbd.x);
kbd.x = 0;
}
}
n = qread(lineq, buf, n);
}
qunlock(&kbd);
poperror();
return n;
case Qscancode:
if(offset == 0)
return readstr(0, buf, n, kscanid);
else
return qread(kscanq, buf, n);
case Qtime:
snprint(tmp, sizeof(tmp), "%.lld", (vlong)mseconds()*1000);
return readstr(offset, buf, n, tmp);
case Qhostowner:
return readstr(offset, buf, n, eve);
case Quser:
return readstr(offset, buf, n, o->user);
case Qjit:
snprint(tmp, sizeof(tmp), "%d", cflag);
return readstr(offset, buf, n, tmp);
case Qnull:
return 0;
case Qmsec:
return readnum(offset, buf, n, TK2MS(MACHP(0)->ticks), NUMSIZE);
case Qsysname:
if(sysname == nil)
return 0;
return readstr(offset, buf, n, sysname);
case Qnotquiterandom:
genrandom(buf, n);
return n;
case Qrandom:
return randomread(buf, n);
case Qmemory:
return poolread(buf, n, offset);
case Qdrivers:
p = malloc(READSTR);
if(p == nil)
error(Enomem);
l = 0;
for(i = 0; devtab[i] != nil; i++)
l += snprint(p+l, READSTR-l, "#%C %s\n", devtab[i]->dc, devtab[i]->name);
if(waserror()){
free(p);
nexterror();
}
n = readstr(offset, buf, n, p);
free(p);
poperror();
return n;
case Qklog:
return qread(klogq, buf, n);
case Qkprint:
rlock(&kprintq);
if(waserror()){
runlock(&kprintq);
nexterror();
}
n = qread(kprintq.q, buf, n);
poperror();
runlock(&kprintq);
return n;
default:
print("consread %llud\n", c->qid.path);
error(Egreg);
}
return -1; /* never reached */
}
void Move(int cLMax, int cRMax)
{
int sL;
int sR;
float tL;
float tR;
float tMax;
int sLSet;
int sRSet;
long ms;
long msLimit;
printf("Move: %d, %d\n", cLMax, cRMax);
if (fAssertEnable)
StartPress();
msLimit = mseconds() + 2000L;
InitEncoders();
if (cLMax < 0)
{
cLMax = -cLMax;
sL = -pwrMax;
}
else
sL = pwrMax;
if (cRMax < 0)
{
cRMax = -cRMax;
sR = -pwrMax;
}
else
sR = pwrMax;
while (cL < cLMax || cR < cRMax)
{
if (stop_button())
break;
ReadEncoders();
if (FBallCapture())
break;
if (fStalled || fBlocked)
{
if (!fForce)
{
beep();beep();beep();
printf("Abort Move\n");
break;
}
ms = mseconds();
if (ms >= msLimit)
{
beep();beep();beep();
printf("Time out\n");
break;
}
}
if (cL >= cLMax)
sL = 0;
if (cR >= cRMax)
sR = 0;
/* printf("L: %d, R: %d\n", cL, cR); */
tL = Parametric(cL, cLMax);
tR = Parametric(cR, cRMax);
sRSet = sR;
sLSet = sL;
/* BUG: Can need less than 3/4 speed - also need not retart balanced
motors when both can track the same! */
if (tL > tR)
sLSet = sL*3/4;
else
sRSet = sR*3/4;
SetMotors(sLSet, sRSet);
}
SetMotors(0,0);
}
