Logo code via bluetooth

I’ve been messing around with implementing a logo interpreter on the sparki. You can send code to it and then execute it… I’m basing it mainly on information found here: sonic.net/~nbs/webturtle/commands/

Currently, I’ve got commands for forward, reverse, right, left, beep, servo, and most the PITA repeat. I’ve also got a remember and go-back command similar to the ‘dead reckoning’ post([url][Sketch] Dead Reckoning]).

So, from a com port to bluetooth window (I’m using a separate arduino install to do this), you could type

repeat 4 forward 10 right 90 next goback beep

and it would draw a box, move back to the start, and then beep…

The upside of this is that you don’t have to plug the sparki in to reprogram it. Just type “reset” and start typing in the next thing to do.

I’m just messing around at the moment, but if anyone had any ideas, let’s hear them… I’d sorta like to make some variable/math parsing, but it may be more than I can do and still keep it simple. I’m also kicking around the idea of having some sort of IFCLOSE statement that uses the ultrasonic to branch, but not sure what the syntax or implementation would be.

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