/* filename: squishydef.tea */ /* Squishy's defines and multi-tasking glue */ #include <aMulti.tea> /* CFG LMT */ #define SRVSTEER 0 /* 0xCF, (0x1A,0x1F) */ #define SRVSPEED 1 /* 0xC0, (0x17,0x18) */ /* servo 0 is for steering */ /* servo 1 is motor controller */ #define DIGBEEP 4 /* digital output for beeper */ #define DIGTUNE 3 /* digital output for tune */ #define APAT 0 /* analog input for pat sensor */ #define NSTEERMIN 1 /* steering limits */ #define NSTEERCTR 128 #define NSTEERMAX 254 /* scooting parameters */ #define NSPDON 208 #define NSPDOFF 128 #define NSPDREV 48 #define NBRAKE 123 #define TMRAMP 55 #define SPDOFFTIME 2000 #define MONAVPROC 1 #define MONAVFILE 0 #define SCOOTPROC 2 #define SCOOTFILE 1 /* cscoot function wrapper */ int cscoot(int nspd, int nstr, int nbrk) { return aMulti_Run13(SCOOTFILE,SCOOTPROC,0,nspd,nstr,nbrk); } /* evade function wrapper */ int evade(int ndir) { return aMulti_Run13(SCOOTFILE,SCOOTPROC,1,ndir,0,0); } /* steer function wrapper */ int steer(int nstr) { return aMulti_Run13(SCOOTFILE,SCOOTPROC,2,nstr,0,0); }

/* filename: squishy0.tea */ /* Squishy's motion control */ #include <aCore.tea> #include <aServo.tea> #include <aDig.tea> #include <aA2D.tea> #include "squishydef.tea" void init() { aDig_Config(DIGBEEP,ADIG_OUTPUT); aDig_Config(DIGTUNE,ADIG_OUTPUT); aServo_SetLimits(SRVSTEER,0x1A1F); aServo_SetLimits(SRVSPEED,0x1718); aServo_SetConfig(SRVSTEER,(unsigned char)0xCF); aServo_SetConfig(SRVSPEED,(unsigned char)0xC0); } /* emit several 50ms beeps */ void beep(int k, int ndur) { int i; for (i=0; i<k; i++) { aDig_Write(DIGBEEP,1); aCore_Sleep(ndur); aDig_Write(DIGBEEP,0); aCore_Sleep(ndur); } } /* toggle a between values b and c (a must equal b or c) */ int toggle(int a, int b, int c) { if (a==b) return c; return b; } void main(char callingProc) { int i; int r; int nback=0; int niter=0; int ndir=0; /* initialize direction and scoot count */ ndir = NSTEERMIN; niter = 0; while (1) { /* steer if scoot has been reset */ if (niter == 0) { steer(ndir); aCore_Sleep(2000); } /* scoot and see if direction changed */ /* if direction hardly changed, back up and turn */ /* (perform evasive maneuver) */ /* then start scooting again */ r = cscoot(NSPDON,ndir,NBRAKE); if (r < 2) { beep(1,200); beep(40,20); beep(60,40); evade(ndir); niter=0; ndir=toggle(ndir,NSTEERMIN,NSTEERMAX); } else { /* change direction after a couple of scoots */ niter++; if (niter==2) { niter=0; ndir=toggle(ndir,NSTEERMIN,NSTEERMAX); } } } }

/* filename: squishy1.tea */ /* Squishy's control routines */ #include <aCore.tea> #include <aServo.tea> #include <aDig.tea> #include "squishydef.tea" /* return absolute value of an int */ int abs(int a) { int val; if (a >= 0) { val = a; } else { val = -a; } return val; } /* read 1 byte from obsolete version of Devantech compass */ /* (not compatible with latest version of compass) */ int read_compass() { int val=0; asm { /* byte 1 is dir (0-255) */ pushlb 0 /* pad byte to short */ pushlb (unsigned char)0xC1 /* IIC address */ pushlb 1 /* bytes to read */ popsm aPortIICRead popss 2 /* store as int */ } return val; } /* send positioning signal to steering servo */ void steer_abs(int npos) { asm { pushss 4 popsm aPortServo+SRVSTEER*aPortServoBlockSize+aOffsetServoAbsolute } } /* send speed value to motor controller */ void speed_abs(int nspd) { asm { pushss 4 popsm aPortServo+SRVSPEED*aPortServoBlockSize+aOffsetServoAbsolute } } /* perform a ramp acceleration */ /* (increases or decreases speed in timed increments) */ void acc(int nspd1, int nspd2, int tmramp) { int i; if (nspd2>nspd1) { for (i=nspd1; i<nspd2; i++) { aCore_Sleep(tmramp); speed_abs(i); } } else { for (i=nspd1; i>=nspd2; i--) { aCore_Sleep(tmramp); speed_abs(i); } } } /* apply reverse speed to stop quickly */ void brake(int nbrk) { acc(128,nbrk,(int)10); acc(nbrk,128,(int)10); } /* IMPLEMENTATION OF EVADE FUNCTION */ /* do some three-point turns in an attempt */ /* to get around an obstacle */ int evade_internal(int ndir) { int i; int tog; aCore_Sleep(500); steer_abs(NSTEERCTR); aCore_Sleep(8000); for (i = 0; i < 2; i++) { acc(NSPDOFF, NSPDREV, 10); acc(NSPDREV, NSPDOFF, 10); aCore_Sleep(500); } tog=ndir; for (i = 0; i < 1; i++) { steer_abs(tog); aCore_Sleep(8000); acc(NSPDOFF, NSPDREV, 10); acc(NSPDREV, NSPDOFF, 10); tog=255-tog; steer_abs(tog); aCore_Sleep(8000); acc(NSPDOFF, NSPDON, 10); acc(NSPDON, NSPDOFF, 10); tog=255-tog; } return 0; } /* IMPLEMENTATION OF CSCOOT FUNCTION */ /* roll in a curve and check compass */ /* no curve (a stall) returns small value */ /* right curve returns positive */ /* left curve returns negative */ int cscoot_internal(int nspd, int nstr, int nbrk) { int n1; int n2; int d; /* take initial heading, drive for a bit, then brake */ n1 = read_compass(); acc(128, nspd, TMRAMP); brake(nbrk); aCore_Sleep(SPDOFFTIME); /* check final heading and calculate direction change */ /* (must handle possible wrap-around with direction) */ n2 = read_compass(); d=(n2-n1); if (d > 128) { d=d-256; } if (d < -128) { d=d+256; } d=abs(d); return d; } /* IMPLEMENTATION OF STEER FUNCTION */ int steer_internal(int nstr) { /* send steering command */ steer_abs(nstr); return 0; } /* CASE STATEMENT TO CHOOSE APPROPRIATE FUNCTION */ void main(char callingProc, char c1, int i1, int i2, int i3) { int retval=0; switch (c1) { case 0: retval = cscoot_internal(i1,i2,i3); break; case 1: retval = evade_internal(i1); break; case 2: retval = steer_internal(i1); break; } aMulti_Halt(callingProc,retval); }

/* filename: squishy2.tea */ /* Squishy's interaction manager */ #include <aCore.tea> #include <aServo.tea> #include <aDig.tea> #include <aA2D.tea> #include <aMulti.tea> #include "squishydef.tea" #define TIMEOUT 100 #define MAXTIMEOUTS 30 #define PETRATE 200 #define PETCT 40 #define PETTHR1 12 void init() { aDig_Config(DIGBEEP,ADIG_OUTPUT); aDig_Config(DIGTUNE,ADIG_OUTPUT); aServo_SetLimits(SRVSTEER,0x1A1F); aServo_SetLimits(SRVSPEED,0x1718); aServo_SetConfig(SRVSTEER,(unsigned char)0xCF); aServo_SetConfig(SRVSPEED,(unsigned char)0xC0); } /* emit several beeps */ void beep(int k, int ndur) { int i; for (i=0; i<k; i++) { aDig_Write(DIGBEEP,1); aCore_Sleep(ndur); aDig_Write(DIGBEEP,0); aCore_Sleep(ndur); } } void buzz(int k, int ndur) { int i; for (i=0; i<k; i++) { aDig_Write(DIGBEEP,1); aCore_Sleep(20); aDig_Write(DIGBEEP,0); aCore_Sleep(ndur); } /* loop to beep and check input switch */ /* (beep indicates robot is turned on) */ void poll_and_beep() { int i; int nflag=1; int nback=0; while (nflag) { /* quit if back is pushed */ for (i = 0; i < 200; i++) { nback = aA2D_ReadInt(APAT); if (nback<250) { nflag=0; break; } } beep(1,50); } } void check_for_some_luvin() { int nback1; int nback0; int ndiff; int nct=0; int ntime=0; int ntimeouts=0; nback0=aA2D_ReadInt(APAT); while (1) { aCore_Sleep(PETRATE); nback1 = aA2D_ReadInt(APAT); ndiff=nback1-nback0; nback0 = aA2D_ReadInt(APAT); if (ndiff>PETTHR1) { nct=nct+1; ntime=0; aDig_Write(DIGBEEP,1); aCore_Sleep(50); aDig_Write(DIGBEEP,0); } ntime=ntime+1; if (ntime>TIMEOUT) { nct=0; ntimeouts=ntimeouts+1; } if (nct>PETCT) break; if (ntimeouts>MAXTIMEOUTS) break; } /* "You Are My Sunshine" lasts 16.4 seconds */ if (nct>PETCT) { aDig_Write(DIGTUNE,1); aCore_Sleep(30000); aCore_Sleep(30000); aCore_Sleep(30000); aCore_Sleep(30000); aCore_Sleep(30000); aCore_Sleep(14000); aDig_Write(DIGTUNE,0); nct=0; ntime=0; } else { /* make annoyed sound */ buzz(16,200); buzz(12,400); buzz(8,600); buzz(4,800); buzz(2,1000); } } void main() { init(); /* wait for back to be pushed */ poll_and_beep(); /* acknowledge start and pause for 1 second */ beep(5,500); aCore_Sleep(10000); while (1) { /* launch the navigation process and give it some time to run */ aMulti_Spawn(MONAVFILE,MONAVPROC); aCore_Sleep(20000); /* wait for back to be pushed */ poll_and_beep(); /* kill navigation process and its subprocess */ aMulti_Kill(MONAVPROC); aMulti_Kill(SCOOTPROC); /* give time for kill to be completed */ aCore_Sleep(100); /* regain control of steering and drive motor */ aServo_SetAbsolute(SRVSTEER,(unsigned char)NSTEERCTR); aServo_SetAbsolute(SRVSPEED,(unsigned char)NSPDOFF); /* if affection received then play a tune */ /* otherwise make an annoyed buzz and go again */ check_for_some_luvin(); } }