I decided to mess with this some more (quite a bit more…).
I was able to implement conditional statements for ‘if less than’ and ‘if greater than’, so I needed a way to do math and query sensor values - hence the stack commands…
a code example (simple avoidance):
repeat 100
ping
dup
iflt 30
forward 10
endif
ifgt 31
right 20
endif
next
all the commands are here: https://sites.google.com/site/jasonelmore/sparkilogo
A warning: I’ve used practically ALL of the program space, so if you’ve got a modified sparki.h, you may need to remove or shorten some of the strings to make it fit. There are a lot of strings at the very bottom and are used to echo back your program, so shortening them shouldn’t cause any trouble at all.
#include <Sparki.h> // include the sparki library
//Line Buffer
#define MAX_BUFF 15
char buff[MAX_BUFF];
byte nbuff=0;
//Program buffer stuff
short prog_buffer_size=10;
short *prog;
byte nprog=0;
byte pprog=0;
boolean running=false;
boolean verbose=true;
boolean immediate_mode=false;
//number stack
short stack_size=10;
short *stack;
short nstack=0;
union //for loop start number and counter... 1-255...
{
//unsigned
byte b[2];
short s;
} u;
//////////////////////////
#define RUN 19118
#define STOP -11792
#define IMMEDIATE -19035
#define RESET 5733
#define LIST 9844
#define UNIT -18124
#define MEM 13485
#define VERBOSE 5698
#define STACK -12253
#define CLEAR -20319
#define POP 16880
#define PUSH 22120
#define DUP 4784
#define ADD 1156
#define SUB 20130
#define MULT -10860
#define DIV 4406
#define POP_STACK -998
#define FORWARD 15959
#define REVERSE 5829
#define RIGHT 9448
#define LEFT 5332
#define PING 9671
#define LSENSE 19630
#define REFLECT 5324
#define BEEP 5296
#define PENDINGREPEAT -999
#define REPEAT 5637
#define NEXT 5908
#define IFLT -26220
#define IFGT -26380
#define ENDIF -18295
#define REMEMBER 5541
#define GOBACK -17343
#define LOOK 15851
#define OPEN -16210
#define CLOSE -19981
#define TONE 15813
int rememberAng=0;
float rememberX=0;
float rememberY=0;
boolean USE_IMPERIAL=false;
float dist_scale=1.0;
void setup()
{
sparki.RGB(RGB_BLUE);
sparki.servo(0);
Serial1.begin(9600);
sparki.clearLCD();
//allocate prog memory
prog = (short *)malloc(sizeof(short) * prog_buffer_size);
if(prog == NULL)
{
sparki.println(F("error: prog alloc fail"));
prog_buffer_size = 0;
}
else
{
sparki.println(F("sparkiLogo!"));
}
stack = (short *)malloc(sizeof(short) * stack_size);
if(stack == NULL)
{
sparki.println(F("error: stack alloc fail"));
stack_size = 0;
}
sparki.print(F("free ram:"));
sparki.print(freeRam());
sparki.updateLCD();
delay(2000);
for(int i=0; i<7; i++)
{
sparki.beep(440+i*440.0/7, 60);
delay(60);
}
Serial1.println(F("READY!"));
}
boolean usesArgument(short cmd)
{
switch(cmd)
{
//place ALL 1 parameter codes here (DOES include REPEAT)
case FORWARD:
case REVERSE:
case RIGHT:
case LEFT:
case LOOK:
case TONE:
case PUSH:
case IFLT:
case IFGT:
case REPEAT:
case LSENSE:
case REFLECT:
return true; break;
default: return false;
}
}
void loop()
{
byte l_nprog = nprog;
while(Serial1.available() > 0)
{
if( (buff[nbuff++] = Serial1.read()) == 13 )
{//CR detected! We need to process the line
buff[nbuff-1] = 0; //get rid of CR and truncate string with NULL
if(nprog>=prog_buffer_size) //MAX_PROG)
{
//Serial1.println(F("resizing prog buf"));
#define BUFFER_GROW_AMT 5
short * new_buffer = (short *)realloc(prog, sizeof(short) * (prog_buffer_size + BUFFER_GROW_AMT));
if(new_buffer != NULL)
{
prog = new_buffer;
prog_buffer_size += BUFFER_GROW_AMT;
Serial1.print(F("prog buffer size:"));
Serial1.println(prog_buffer_size);
}
else
{
Serial1.println(F("prog buf not be resized!"));
break;
}
}
//break token
char * s; //used by strtok to keep progress...
char * p = strtok_r(buff," ",&s);
char * arg = strtok_r(NULL," ",&s);
short argVal=0;
if(arg != NULL)
{
if((arg[0] == 'P') || (arg[0] == 'p'))
{
//Serial1.print(F("Using stack..."));
argVal = POP_STACK;
}
else
argVal = atoi(arg);
}
//check for keywords
short code = commandCode(p);
if(usesArgument(code) && (code != REPEAT))
{
prog[nprog++] = code;
prog[nprog++] = argVal;
Serial1.print(F("OK:"));
Serial1.print(lookupCode(code));
Serial1.print(F(" "));
Serial1.println(argVal);
}
else
switch(code)
{
case 0:
Serial1.println(F("Yes?"));
break;
case RUN:
if(nprog>0)
{
Serial1.println(F("RUN..."));
running=true;
}
break;
case STOP:
Serial1.println(F("STOP."));
running=false;
pprog = 0;
break;
case RESET:
Serial1.println(F("RESET"));
nprog=0;
pprog=0;
running=false;
break;
case UNIT:
USE_IMPERIAL = !USE_IMPERIAL;
if(USE_IMPERIAL)
{
dist_scale = 2.54;
Serial1.println(F("distances in inches"));
}
else
{
dist_scale = 1.0;
Serial1.println(F("distances in cm"));
}
break;
case MEM:
Serial1.print(F("Memory free:"));
Serial1.println(freeRam());
break;
case VERBOSE:
verbose = !verbose;
Serial1.print(F("Verbose:"));
if(verbose) Serial1.println(F("ON"));
else Serial1.println(F("OFF"));
break;
case IMMEDIATE:
immediate_mode = !immediate_mode;
Serial1.print(F("Immediate Mode:"));
if(immediate_mode) Serial1.println(F("ON"));
else Serial1.println(F("OFF"));
break;
case STACK:
{
Serial1.print(F("Stack size:"));
Serial1.println(nstack);
int i=nstack-1;
while(i>=0)
{
Serial1.print(i);
Serial1.print(F(" : "));
Serial1.println(stack[i]);
i--;
}
}
break;
//NOTE:
// usesArgument(code)
//place all 'normal' 1 parameter codes here (doesn't include REPEAT)
/* case FORWARD:
case REVERSE:
case RIGHT:
case LEFT:
case LOOK:
case TONE:
case PUSH:
case IFLT:
case IFGT:
prog[nprog++] = code;
prog[nprog++] = argVal;
Serial1.print(F("OK:"));
Serial1.print(p);
Serial1.print(F(" "));
Serial1.println(argVal);
break;
*/
//place all no parameter codes here
case BEEP:
case CLOSE:
case OPEN:
case REMEMBER:
case GOBACK:
case CLEAR:
case POP:
case DUP:
case ADD:
case SUB:
case MULT:
case DIV:
case PING:
case ENDIF:
prog[nprog++] = code;
Serial1.print(F("OK:"));
Serial1.println(lookupCode(code));
if(immediate_mode)
{
pprog = l_nprog;
running = true;
}
break;
case REPEAT:
{
short b = atoi(arg);
if(b>0)
{
if(b<=255)
{
Serial1.print(F("pending repeat "));
Serial1.print(b);
Serial1.println(F(" times"));
prog[nprog++] = PENDINGREPEAT;
u.b[0]=0; u.b[1]=b;
prog[nprog++] = u.s;//((short)(b))<<8; //high byte is number, low is accum
}
else Serial1.println(F("repeat must be less than 255"));
}
else Serial1.println(F("use 'repeat (how many times)'"));
}
break;
case NEXT:
{
//Serial1.println(F("next cmd"));
prog[nprog++] = NEXT;
int t=nprog-3;
bool found=false;
while(t >= 0 && !found)
{
if(prog[t] != PENDINGREPEAT) t--;
else
{
Serial1.print(F(" found repeat at ")); Serial1.println(t);
prog[nprog++] = t;
prog[t] = REPEAT;
found = true;
}
}
if(!found)
{
Serial1.println(F(" repeat NOT found"));
nprog--;
}
}
break;
case LIST:
{
int t=0;
boolean flagArg=false;
Serial1.println(F("Current program:"));
while(t<nprog)
{
Serial1.print(t);
Serial1.print(F(" : "));
short cmd=prog[t++];
if(flagArg)
{
Serial1.print(F("(A) "));
Serial1.println(cmd);
}
else
{
Serial1.print(F("("));
Serial1.print(cmd);
Serial1.print(F(") "));
Serial1.println(lookupCode(cmd));
}
flagArg=false;
if(usesArgument(cmd) || (cmd==NEXT)) flagArg=true; //this will make the upcoming code show as argument...
}
Serial1.println(F("END"));
}
break;
default:
Serial1.print(F("I don't know how to '"));
Serial1.print(p);
Serial1.print(F("' code:"));
Serial1.println(code);
}
//we're done, clear buffer...
nbuff=0;
}
if(nbuff>=MAX_BUFF) {nbuff=MAX_BUFF-1; Serial1.println(F("ERR:Buff Overflow"));}
}
if(running)
{ //Do program
sparki.RGB(RGB_GREEN);
Serial1.print(pprog);
short cmd = prog[pprog++];
short argVal=0;
short *p_argVal = &argVal;
//switch(cmd) //if argument needed, go ahead and get it...
if(usesArgument(cmd) || (cmd==NEXT))
{
if(prog[pprog] == POP_STACK)
{
if(nstack>0)
argVal = stack_pop();
else
Serial1.print(F("stack empty!"));
}
else
{
argVal = prog[pprog];
p_argVal = &prog[pprog]; //pointer to mem for REPEAT to work right...
}
pprog++;
Serial1.print(F(" : (")); Serial1.print(cmd); Serial1.print(F(") "));
Serial1.print(lookupCode(cmd)); Serial1.print(F(" arg:")); Serial1.println(argVal);
}
else
{
Serial1.print(F(" : (")); Serial1.print(cmd); Serial1.print(F(") "));
Serial1.println(lookupCode(cmd));
}
//Serial1.println(F("prog loop start..."));
switch(cmd)
{
case FORWARD: rememberX += dist_scale*argVal*sin(rememberAng*6.28/360.0);
rememberY += dist_scale*argVal*cos(rememberAng*6.28/360.0);
sparki.moveForward(dist_scale*(float)argVal);
break;
case REVERSE: rememberX += dist_scale*argVal*sin(rememberAng*6.28/360.0);
rememberY += dist_scale*argVal*cos(rememberAng*6.28/360.0);
sparki.moveBackward(dist_scale*(float)argVal);
break;
case LEFT: rememberAng -= argVal;
sparki.moveLeft((float)argVal);
break;
case RIGHT: rememberAng += argVal;
sparki.moveRight((float)argVal);
break;
case BEEP: sparki.beep();
break;
case LOOK:
sparki.servo((float)argVal);
break;
case TONE:
sparki.beep(argVal);
break;
case OPEN:
sparki.gripperOpen();
delay(4800);
sparki.gripperStop();
break;
case CLOSE:
sparki.gripperClose();
delay(4800);
sparki.gripperStop();
break;
case REPEAT:
{ //copy high byte to low byte
u.s = *p_argVal;
u.b[0] = u.b[1];
*p_argVal = u.s;
if(verbose){Serial1.print(F(" Starting Repeat: ")); Serial1.println(u.b[1]);}
} break;
case NEXT:
{
int t = argVal;//prog[pprog++]; //place to jump to if needed
u.s = prog[t+1]; //repeat counter value
if((--u.b[0]) > 0) //we need to keep looping
{
pprog = t+2;
if(verbose){Serial1.print(F(" Repeating...jump to ")); Serial1.println(pprog);}
}
else
if(verbose)Serial1.println(F("Done Repeating."));
prog[t+1] = u.s;
} break;
case REMEMBER: rememberX = 0; rememberY = 0.0; rememberAng=0;
break;
case GOBACK:
{
//don't need to increment, no arg... pprog++;
//if(verbose)Serial1.println(F("Going Back..."));
while(rememberAng<0) rememberAng+=360;
while(rememberAng>=360) rememberAng-=360;
if(rememberAng>0) sparki.moveLeft(rememberAng);
if(rememberY>0)
sparki.moveBackward(rememberY);
else if(rememberY<0)
sparki.moveForward(-rememberY);
sparki.moveLeft(90);
if(rememberX>0)
sparki.moveForward(rememberX);
else if(rememberX<0)
sparki.moveBackward(-rememberX);
sparki.moveRight(90);
rememberX = 0;
rememberY = 0;
rememberAng = 0;
//if(verbose)Serial1.println(F("I'm BACK!"));
} break;
//stack stuff
case CLEAR:
nstack = 0;
if(verbose)Serial1.println(F("stack cleared"));
break;
case POP:
//if(verbose)Serial1.print(F("stack pop:"));
if(nstack>0)
{
short n = stack_pop();
Serial1.println(n);
}
else
//if(verbose)Serial1.println(F("empty"));
break;
case PUSH:
{
short n = stack_push(argVal);
//Serial1.print(F("stack push:"));
//Serial1.println(n);
} break;
case DUP:
if(nstack>0)
{
//if(verbose)Serial1.print(F("stack dup:"));
short n1 = stack_pop();
stack_push(n1);
stack_push(n1);
//if(verbose)Serial1.println(n1);
}
break;
case ADD:
if(nstack>1)
{
//if(verbose)Serial1.print(F("stack add:"));
short n1 = stack_pop();
short n2 = stack_pop();
n2 = stack_push(n2 + n1);
//if(verbose)Serial1.println(n2);
}
break;
case SUB:
if(nstack>1)
{
//if(verbose)Serial1.print(F("stack sub:"));
short n1 = stack_pop();
short n2 = stack_pop();
n2 = stack_push(n2 - n1);
//if(verbose)Serial1.println(n2);
}
break;
case MULT:
if(nstack>1)
{
//if(verbose)Serial1.print(F("stack mult:"));
short n1 = stack_pop();
short n2 = stack_pop();
n2 = stack_push(n2 * n1);
//if(verbose)Serial1.println(n2);
}
break;
case DIV:
if(nstack>1)
{
//if(verbose)Serial1.print(F("stack div:"));
short n1 = stack_pop();
short n2 = stack_pop();
n2 = stack_push(n2 / n1);
//if(verbose)Serial1.println(n2);
}
break;
case PING:
stack_push(sparki.ping());
break;
case LSENSE:
if(argVal == -1) stack_push(sparki.lightLeft());
else if(argVal == +1) stack_push(sparki.lightRight());
else stack_push(sparki.lightCenter());
break;
case REFLECT:
if(argVal < -1) stack_push(sparki.edgeLeft());
else if(argVal == -1) stack_push(sparki.lineLeft());
else if(argVal > 1) stack_push(sparki.edgeRight());
else if(argVal == 1) stack_push(sparki.lineRight());
else stack_push(sparki.lineCenter());
break;
case IFLT:
case IFGT:
{
short skipover = 0;
short testval = stack_pop();
//if(verbose)Serial1.print(F("IF "));
//if(verbose)Serial1.print(argVal);
if(cmd==IFLT)
{
//if(verbose)Serial1.print(F("<"));
if(argVal >= testval) skipover=1;
}
if(cmd==IFGT)
{
//if(verbose)Serial1.print(F(">"));
if(argVal <= testval) skipover=1;
}
//if(verbose)Serial1.println(testval);
if(skipover>0)
{
if(verbose) Serial1.print(F("condition false, skipping to line "));
while(skipover)
{
short nextCmd = prog[pprog];
if(nextCmd != ENDIF)
{
if((nextCmd == IFLT)||(nextCmd == IFGT)) //if we hit another IF, we need to skip it's endif
skipover++;
}
else
skipover--;
if(skipover>0)
{
if(usesArgument(nextCmd))
pprog+=2;
else
pprog++;
}
if(pprog>=nprog) //something's wrong, quit...
{
//if(verbose)Serial1.print(F("(error)"));
skipover=0;
}
}
Serial1.println(pprog);
}
else ;//if(verbose) Serial1.println(F("condition true"));
}
break;
case ENDIF:
//if(verbose) Serial1.println(F("EndIf"));
break;
default:
Serial1.print(F("unknown command:"));
Serial1.println(-cmd);
}
if(pprog >= nprog)
{
pprog = 0;
running = false;
if(verbose)Serial1.println(F("End of Program"));
}
//Serial1.println(F("prog loop stop..."));
}
else
{
sparki.RGB(RGB_GREEN);
}
sparki.clearLCD();
if(running) sparki.println(F(" running"));
else sparki.println(F(" idle"));
sparki.print(F("program:"));
sparki.print(pprog);
sparki.print(F("/"));
sparki.println(nprog);
sparki.updateLCD();
delay(10);//probably should take out...
}
short commandCode(char *p)
{//looks at the 1st 4 characters, also
byte i=0;
short code=0;
byte c;
while( (*(p+i)>0) && (i<4) )
{
c = *(p+i);
if(c>96) c-=32 ; //adjust lower to uppercase
if(c>64)
{
//Serial1.println(c);
code = (code<<5) | (c-64);
}
i++;
}
return code;
}
short stack_push(short n)
{
if(nstack+1 > stack_size)
{
short * new_buffer = (short *)realloc(stack, sizeof(short) * (stack_size + BUFFER_GROW_AMT));
if(new_buffer != NULL)
{
stack = new_buffer;
stack_size += BUFFER_GROW_AMT;
}
}
stack[nstack++] = n;
return n;
}
short stack_pop(void)
{
if(nstack>0) return(stack[--nstack]);
return(0);
}
int freeRam () {
extern int __heap_start, *__brkval;
int v;
return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
}
const __FlashStringHelper* lookupCode(short code)
{
switch(code)
{
//first group never get put into program... so they aren't needed here
/*
case RUN : return(F("RUN"));
case STOP : return(F("STOP"));
case RESET : return(F("RESET"));
case LIST : return(F("LIST"));
case UNIT : return(F("UNIT"));
case MEM : return(F("MEM"));
*/
case CLEAR : return(F("CLEAR"));
case POP : return(F("POP"));
case PUSH : return(F("PUSH"));
case DUP : return(F("DUP"));
case ADD : return(F("ADD"));
case SUB : return(F("SUB"));
case MULT : return(F("MULT"));
case DIV : return(F("DIV"));
// case POP_STACK : return(F("POP_STACK"));
case FORWARD : return(F("FORWARD"));
case REVERSE : return(F("REVERSE"));
case RIGHT : return(F("RIGHT"));
case LEFT : return(F("LEFT"));
case PING : return(F("PING"));
case BEEP : return(F("BEEP"));
case PENDINGREPEAT: return(F("PEND_RPT"));
case REPEAT : return(F("REPEAT"));
case NEXT : return(F("NEXT"));
case IFLT : return(F("IFLT"));
case IFGT : return(F("IFGT"));
case ENDIF : return(F("ENDIF"));
case REMEMBER : return(F("REMEMBER"));
case GOBACK : return(F("GOBACK"));
case LOOK : return(F("LOOK"));
case OPEN : return(F("OPEN"));
case CLOSE : return(F("CLOSE"));
case TONE : return(F("TONE"));
case LSENSE : return(F("LSENSE"));
case REFLECT : return(F("REFLECT"));
}
return(F("command not found"));
}