added a bunch of robot functions to SURVEYOR_HOST configuration:

signal(), input(), sonar(), range(), color(), blob(), imgcap(), 
    imgrcap(), imgdiff(), compass(), readi2c(), readi2c2(), writei2c()
to the original set of functions:
    motors(), delay(), laser(), servo(), servo2(), time(), rand()



git-svn-id: http://picoc.googlecode.com/svn/trunk@139 21eae674-98b7-11dd-bd71-f92a316d2d60

platform_library.c

@@ -13,18 +13,25 @@ void PrintInteger(struct ParseState *Parser, struct Value *ReturnValue, struct V
 #ifdef UNIX_HOST
 void Random(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
 {
-    ReturnValue->Val->Integer = rand();
+    ReturnValue->Val->Integer = random();
 }
 #endif
 
 static int SomeVar = 42;
 static int SomeArray[4];
+static int Blobcnt, Blobx1, Blobx2, Bloby1, Bloby2;
 void PlatformLibraryInit()
 {
     struct ValueType *IntArrayType;
 
     VariableDefinePlatformVar(NULL, "somevar", &IntType, (union AnyValue *)&SomeVar, TRUE);
 
+    VariableDefinePlatformVar(NULL, "blobcnt", &IntType, (union AnyValue *)&Blobcnt, FALSE);
+    VariableDefinePlatformVar(NULL, "blobx1", &IntType, (union AnyValue *)&Blobx1, FALSE);
+    VariableDefinePlatformVar(NULL, "blobx2", &IntType, (union AnyValue *)&Blobx2, FALSE);
+    VariableDefinePlatformVar(NULL, "bloby1", &IntType, (union AnyValue *)&Bloby1, FALSE);
+    VariableDefinePlatformVar(NULL, "bloby2", &IntType, (union AnyValue *)&Bloby2, FALSE);
+    
     IntArrayType = TypeGetMatching(NULL, &IntType, TypeArray, 4, NULL);
     SomeArray[0] = 12;
     SomeArray[1] = 34;
@@ -34,11 +41,39 @@ void PlatformLibraryInit()
 }
 
 #ifdef SURVEYOR_HOST
+extern unsigned int vblob(unsigned char *, unsigned char *, unsigned int);
+extern unsigned int vpix(unsigned char *, unsigned int, unsigned int);
+extern void init_colors();
+extern void vhist(unsigned char *);
+extern void vmean(unsigned char *);
+extern void color_segment(unsigned char *);
+extern void edge_detect(unsigned char *, unsigned char *, int);
+extern void i2cwrite(unsigned char, unsigned char *, unsigned int, int);
+extern void i2cread(unsigned char, unsigned char *, unsigned int, int);
+
 extern int pwm1_mode, pwm2_mode, pwm1_init, pwm2_init;
 extern int lspeed, rspeed, lspeed2, rspeed2, base_speed;
 extern int sonar_data[];
 extern int imgWidth, imgHeight, frame_diff_flag;
 
+extern unsigned int ymax[], ymin[], umax[], umin[], vmax[], vmin[];
+extern unsigned int blobx1[], blobx2[], bloby1[], bloby2[], blobcnt[], blobix[];
+extern unsigned int hist0[], hist1[], hist2[], mean[];
+
+void Csignal(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)  // check for kbhit, return t or nil
+{
+    unsigned char ch;
+    if (getchar(&ch))
+        ReturnValue->Val->Integer = 1;
+    else
+        ReturnValue->Val->Integer = 0;
+}
+
+void Cinput(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)  // return 0-9 from console input
+{
+    ReturnValue->Val->Integer = getch() & 0x0F;
+}
+
 void Cdelay(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
 {
     int del;
@@ -72,7 +107,6 @@ void Cmotors(struct ParseState *Parser, struct Value *ReturnValue, struct Value
         pwm1_init = 1;
         pwm1_mode = PWM_PWM;
         base_speed = 50;
-        lspeed = rspeed = 0;
     }
     setPWM(lspeed, rspeed);
 }
@@ -121,233 +155,128 @@ void Claser(struct ParseState *Parser, struct Value *ReturnValue, struct Value *
         lasers_off();
 }
 
-/*
+extern int sonar_data[];
-void imgcap()
+void Csonar(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // read sonar module
 {
-    start_check();
+    unsigned int i;
-    grab_frame();
+    i = Param[0]->Val->Integer;
-    finish_check();
+    if ((i<1) || (i>4)) {
+        ReturnValue->Val->Integer = 0;
+        return;
+    }
+    ping_sonar();
+    ReturnValue->Val->Integer = sonar_data[i] / 100;
 }
 
-void imgrcap()
+void Ccolor(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // set color bin - 
+                //    color (color, ymin, ymax, umin, umax, vmin, vmax);                  
 {
-    start_check();
+    int ix;
-    grab_reference_frame();
+    
-    finish_check();
+    ix = Param[0]->Val->Integer;
+    ymin[ix] = Param[1]->Val->Integer;
+    ymax[ix] = Param[2]->Val->Integer;
+    umin[ix] = Param[3]->Val->Integer;
+    umax[ix] = Param[4]->Val->Integer;
+    vmin[ix] = Param[5]->Val->Integer;
+    vmax[ix] = Param[6]->Val->Integer;
 }
 
-void imgdiff()
+void Crange(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
 {
-    start_check();
+    ReturnValue->Val->Integer = laser_range(0);
-    eval_exp(&frame_diff_flag);
-    finish_check();
 }
 
+void Cimgcap(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
+{
+    grab_frame();   // capture frame for processing
+}
 
-int blob()    //    search for blob by color, index;  return center point X,Y and width Z
+void Cimgrcap(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
+{
+    grab_reference_frame();   // capture reference frame for differencing
+}
+
+void Cimgdiff(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
+{
+    frame_diff_flag = Param[0]->Val->Integer;   // set/clear frame_diff_flag
+}
+
+void Ccompass(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)  // return reading from HMC6352 I2C compass
+{
+    unsigned char i2c_data[2];
+    unsigned int ix;
+    
+    i2c_data[0] = 0x41;  // read compass twice to clear last reading
+    i2cread(0x21, (unsigned char *)i2c_data, 2, SCCB_ON);
+    i2c_data[0] = 0x41;
+    i2cread(0x21, (unsigned char *)i2c_data, 2, SCCB_ON);
+    ix = ((i2c_data[0] << 8) + i2c_data[1]) / 10;
+    ReturnValue->Val->Integer = ix;
+}
+
+void Creadi2c(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)  //  syntax   val = readi2c(device, register);
+{
+    unsigned char i2c_device, i2c_data[2];
+    
+    i2c_device = (unsigned char)Param[0]->Val->Integer;
+    i2c_data[0] = (unsigned char)Param[1]->Val->Integer;
+    
+    i2cread(i2c_device, (unsigned char *)i2c_data, 1, SCCB_OFF);
+    ReturnValue->Val->Integer = ((int)i2c_data[0] & 0x000000FF);
+}
+
+void Creadi2c2(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)  //  syntax   two_byte_val = readi2c(device, register); 
+{
+    unsigned char i2c_device, i2c_data[2];
+
+    i2c_device = (unsigned char)Param[0]->Val->Integer;
+    i2c_data[0] = (unsigned char)Param[1]->Val->Integer;
+    
+    i2cread(i2c_device, (unsigned char *)i2c_data, 2, SCCB_OFF);
+    ReturnValue->Val->Integer = (((unsigned int)i2c_data[0] << 8) + i2c_data[1]);
+}
+
+void Cwritei2c(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)  //  syntax   writei2c(device, register, value);
+{
+    unsigned char i2c_device, i2c_data[2];
+
+    i2c_device = (unsigned char)Param[0]->Val->Integer;
+    i2c_data[0] = (unsigned char)Param[1]->Val->Integer;
+    i2c_data[1] = (unsigned char)Param[2]->Val->Integer;
+    
+    i2cwrite(i2c_device, (unsigned char *)i2c_data, 1, SCCB_OFF);
+}
+
+void Cblob(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)    //    search for blob by color, index;  return center point X,Y and width Z
 {
     int ix, iblob;
 
-    start_check();
+    ix = Param[0]->Val->Integer;
-    eval_exp(&ix);
+    iblob = Param[1]->Val->Integer;
-    if ((ix < 0) || (ix > 9))
-        sntx_err(PARAM_ERR);
-
-    get_token(); // look for comma separator 
-    if(*token != ',')
-        sntx_err(SYNTAX); 
-
-    eval_exp(&iblob);
         
     if (iblob == 0)
         vblob((unsigned char *)FRAME_BUF, (unsigned char *)FRAME_BUF3, ix);
 
     if (blobcnt[iblob] == 0) {
-        assign_var("x", 0); 
+        Blobcnt = 0;
-        assign_var("y", 0); 
-        assign_var("z", 0); 
     } else {
-        assign_var("x", (blobx1[iblob] + blobx2[iblob]) / 2); 
+        Blobcnt = blobcnt[iblob];
-        assign_var("y", (bloby1[iblob] + bloby2[iblob]) / 2); 
+        Blobx1 = blobx1[iblob];
-        assign_var("z", (blobx2[iblob] - blobx1[iblob]) + 1); 
+        Blobx2 = blobx2[iblob];
+        Bloby1 = bloby1[iblob];
+        Bloby2 = bloby2[iblob];
     }
-    finish_check();
+    ReturnValue->Val->Integer = blobcnt[iblob];
-    return blobcnt[iblob];
 }
 
-int readi2c()  //  syntax   val = readi2c(device, register);
-{
-    unsigned char i2c_device, i2c_data[2];
-    int itmp;
-    
-    start_check();
-    eval_exp(&itmp);
-    i2c_device = (unsigned char)itmp;
-    
-    get_token(); // look for comma separator 
-    if(*token != ',')
-        sntx_err(SYNTAX); 
-
-    eval_exp(&itmp);
-    i2c_data[0] = (unsigned char)itmp;
-    
-    i2cread(i2c_device, (unsigned char *)i2c_data, 1, SCCB_OFF);
-    finish_check();
-    return ((int)i2c_data[0] & 0x000000FF);
-}
-
-int readi2c2()  //  syntax   two_byte_val = readi2c(device, register); 
-{
-    unsigned char i2c_device, i2c_data[2];
-    int itmp;
-    
-    start_check();
-    eval_exp(&itmp);
-    i2c_device = (unsigned char)itmp;
-    
-    get_token(); // look for comma separator 
-    if(*token != ',')
-        sntx_err(SYNTAX); 
-
-    eval_exp(&itmp);
-    i2c_data[0] = (unsigned char)itmp;
-    
-    i2cread(i2c_device, (unsigned char *)i2c_data, 2, SCCB_OFF);
-    finish_check();
-    return (((unsigned int)i2c_data[0] << 8) + i2c_data[1]);
-}
-
-void writei2c()  //  syntax   writei2c(device, register, value);
-{
-    unsigned char i2c_device, i2c_data[2];
-    int itmp;
-    
-    start_check();
-    eval_exp(&itmp);
-    i2c_device = (unsigned char)itmp;
-    
-    get_token(); // look for comma separator 
-    if(*token != ',')
-        sntx_err(SYNTAX); 
-
-    eval_exp(&itmp);
-    i2c_data[int crand()
-{
-    start_check();
-    finish_check();
-    return (int)rand();
-}
-
-0] = (unsigned char)itmp;
-    
-    get_token(); // look for comma separator 
-    if(*token != ',')
-        sntx_err(SYNTAX); 
-
-    eval_exp(&itmp);
-    i2c_data[1] = (unsigned char)itmp;
-    
-    i2cwrite(i2c_device, (unsigned char *)i2c_data, 1, SCCB_OFF);
-    finish_check();
-}
-
-int sonar()  //  syntax   val = sonar(channel);
-{
-    int i1;
-   
-    ping_sonar();
-    start_check();
-    eval_exp(&i1);
-    
-    finish_check();
-    return sonar_data[i1];
-}
-
-void color() // set color bin - 
-                //    color (color, ymin, ymax, umin, umax, vmin, vmax);                  
-{
-  int ix, y1, y2, u1, u2, v1, v2;
- 
-  start_check();
-  eval_exp(&ix);
-  if ((ix < 0) || (ix > 9))
-    sntx_err(PARAM_ERR);
-  get_token(); // look for comma separator 
-  if(*token != ',')
-    sntx_err(SYNTAX); // syntax error 
-  eval_exp(&y1);
-  if ((y1 < 0) || (y1 > 255))
-    sntx_err(PARAM_ERR);
-  get_token(); // look for comma separator 
-  if(*token != ',')
-    sntx_err(SYNTAX); // syntax error 
-  eval_exp(&y2);
-  if ((y2 < 0) || (y2 > 255))
-    sntx_err(PARAM_ERR);
-  get_token(); // look for comma separator 
-  if(*token != ',')
-    sntx_err(SYNTAX); // syntax error 
-  eval_exp(&u1);
-  if ((u1 < 0) || (u1 > 255))
-    sntx_err(PARAM_ERR);
-  get_token(); // look for comma separator 
-  if(*token != ',')
-    sntx_err(SYNTAX); // syntax error 
-  eval_exp(&u2);
-  if ((u2 < 0) || (u2 > 255))
-    sntx_err(PARAM_ERR);
-  get_token(); // look for comma separator 
-  if(*token != ',')
-    sntx_err(SYNTAX); // syntax error 
-  eval_exp(&v1);
-  if ((v1 < 0) || (v1 > 255))
-    sntx_err(PARAM_ERR);
-  get_token(); // look for comma separator 
-  if(*token != ',')
-    sntx_err(SYNTAX); // syntax error 
-  eval_exp(&v2);
-  if ((v2 < 0) || (v2 > 255))
-    sntx_err(PARAM_ERR);
-
-  ymin[ix] = y1;
-  ymax[ix] = y2;
-  umin[ix] = u1;
-  umax[ix] = u2;
-  vmin[ix] = v1;
-  vmax[ix] = v2;
-  finish_check();
-}
-
-void resolution()
-{
-    int target;
-    start_check();
-    eval_exp(&target);
-    if(target == 'a'){
-        camera_reset(160);
-    } else if (target == 'b'){ 
-        camera_reset(320);
-    } else if (target == 'c'){
-        camera_reset(640);
-    } else if (target == 'A'){
-        camera_reset(1280);
-    }
-    finish_check();
-}
-
-int crange()
-{
-    start_check();
-    finish_check();
-    return (int)laser_range(0);
-}
-*/
-
 /* list of all library functions and their prototypes */
 struct LibraryFunction PlatformLibrary[] =
 {
     { SayHello,     "void sayhello()" },
     { PrintInteger, "void printint(int)" },
+    { Csignal,      "int signal()" },
+    { Cinput,       "int input()" },
     { Cdelay,       "void delay(int)" },
     { Crand,        "int rand(int)" },
     { Ctime,        "int time()" },
@@ -355,6 +284,17 @@ struct LibraryFunction PlatformLibrary[] =
     { Cservo,       "void servo(int, int)" },
     { Cservo2,      "void servo2(int, int)" },
     { Claser,       "void laser(int)" },
+    { Csonar,       "void sonar(int)" },
+    { Crange,       "void range(int)" },
+    { Ccolor,       "void color(int, int, int, int, int, int, int)" },
+    { Cblob,         "int blob(int, int)" },
+    { Cimgcap,      "void imgcap()" },
+    { Cimgrcap,     "void imgrcap()" },
+    { Cimgdiff,     "void imgdiff(int)" },
+    { Ccompass,     "int compass(int)" },
+    { Creadi2c,     "int readi2c(int, int)" },
+    { Creadi2c2,    "int readi2c2(int, int)" },
+    { Cwritei2c,    "void writei2c(int, int, int)" },
     { NULL,         NULL }
 };
 #endif