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