* added vpix(), vmean(), asin(), acos()

renamed servo() to servos(), servo2() to servos2(), color() to vcolor(),
    imgcap() to vcap(), imgrcap() to vrcap(), imgdiff() to vdiff(), 
    blob() to vblob()



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

platform_library.c

@@ -19,7 +19,7 @@ void Random(struct ParseState *Parser, struct Value *ReturnValue, struct Value *
 
 static int SomeVar = 42;
 static int SomeArray[4];
-static int Blobcnt, Blobx1, Blobx2, Bloby1, Bloby2;
+static int Blobcnt, Blobx1, Blobx2, Bloby1, Bloby2, Iy1, Iy2, Iu1, Iu2, Iv1, Iv2;
 void PlatformLibraryInit()
 {
     struct ValueType *IntArrayType;
@@ -31,6 +31,12 @@ void PlatformLibraryInit()
     VariableDefinePlatformVar(NULL, "blobx2", &IntType, (union AnyValue *)&Blobx2, FALSE);
     VariableDefinePlatformVar(NULL, "bloby1", &IntType, (union AnyValue *)&Bloby1, FALSE);
     VariableDefinePlatformVar(NULL, "bloby2", &IntType, (union AnyValue *)&Bloby2, FALSE);
+    VariableDefinePlatformVar(NULL, "y1", &IntType, (union AnyValue *)&Iy1, FALSE);
+    VariableDefinePlatformVar(NULL, "y2", &IntType, (union AnyValue *)&Iy2, FALSE);
+    VariableDefinePlatformVar(NULL, "u1", &IntType, (union AnyValue *)&Iu1, FALSE);
+    VariableDefinePlatformVar(NULL, "u2", &IntType, (union AnyValue *)&Iu2, FALSE);
+    VariableDefinePlatformVar(NULL, "v1", &IntType, (union AnyValue *)&Iv1, FALSE);
+    VariableDefinePlatformVar(NULL, "v2", &IntType, (union AnyValue *)&Iv2, FALSE);
     
     IntArrayType = TypeGetMatching(NULL, &IntType, TypeArray, 4, NULL);
     SomeArray[0] = 12;
@@ -80,10 +86,10 @@ void Cmotors(struct ParseState *Parser, struct Value *ReturnValue, struct Value
 {
     lspeed = Param[0]->Val->Integer;
     if ((lspeed < -100) || (lspeed > 100))
-        ProgramFail(NULL, "motors():  bad parameter");
+        ProgramFail(NULL, "motors():  left motor value out of range");
     rspeed = Param[1]->Val->Integer;
     if ((rspeed < -100) || (rspeed > 100))
-        ProgramFail(NULL, "motors():  bad parameter");
+        ProgramFail(NULL, "motors():  right motor value out of range");
     if (!pwm1_init) {
         initPWM();
         pwm1_init = 1;
@@ -93,16 +99,16 @@ void Cmotors(struct ParseState *Parser, struct Value *ReturnValue, struct Value
     setPWM(lspeed, rspeed);
 }
 
-void Cservo(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
+void Cservos(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
 {
     int lspeed, rspeed;
     
     lspeed = Param[0]->Val->Integer;
     if ((lspeed < 0) || (lspeed > 100))
-        ProgramFail(NULL, "servo():  bad parameter");
+        ProgramFail(NULL, "servos():  TMR2 value out of range");
     rspeed = Param[1]->Val->Integer;
     if ((rspeed < 0) || (rspeed > 100))
-        ProgramFail(NULL, "servo()():  bad parameter");
+        ProgramFail(NULL, "servos()():  TMR3 value out of range");
     if (!pwm1_init) {
         initPPM1();
         pwm1_init = 1;
@@ -111,16 +117,16 @@ void Cservo(struct ParseState *Parser, struct Value *ReturnValue, struct Value *
     setPPM1(lspeed, rspeed);
 }
 
-void Cservo2(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
+void Cservos2(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
 {
     int lspeed, rspeed;
     
     lspeed = Param[0]->Val->Integer;
     if ((lspeed < 0) || (lspeed > 100))
-        ProgramFail(NULL, "servo2():  bad parameter");
+        ProgramFail(NULL, "servos2():  TMR6 value out of range");
     rspeed = Param[1]->Val->Integer;
     if ((rspeed < 0) || (rspeed > 100))
-        ProgramFail(NULL, "servo2():  bad parameter");
+        ProgramFail(NULL, "servos2():  TMR7 value out of range");
     if (!pwm2_init) {
         initPPM2();
         pwm2_init = 1;
@@ -145,20 +151,24 @@ void Claser(struct ParseState *Parser, struct Value *ReturnValue, struct Value *
     }
 }
 
-extern int sonar_data[];
 void Csonar(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // read sonar module
 {
     unsigned int i;
     i = Param[0]->Val->Integer;
     if ((i<1) || (i>4)) {
-        ProgramFail(NULL, "sonar():  bad parameter");
+        ProgramFail(NULL, "sonar():  1, 2, 3, 4 are only valid selections");
     }
     ping_sonar();
     ReturnValue->Val->Integer = sonar_data[i] / 100;
 }
 
-void Ccolor(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // set color bin - 
+void Crange(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
-                //    color (color, ymin, ymax, umin, umax, vmin, vmax);                  
+{
+    ReturnValue->Val->Integer = laser_range(0);
+}
+
+void Cvcolor(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // set color bin - 
+                //    vcolor (color, ymin, ymax, umin, umax, vmin, vmax);                  
 {
     int ix;
     
@@ -171,26 +181,65 @@ void Ccolor(struct ParseState *Parser, struct Value *ReturnValue, struct Value *
     vmax[ix] = Param[6]->Val->Integer;
 }
 
-void Crange(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
+void Cvcap(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
-{
-    ReturnValue->Val->Integer = laser_range(0);
-}
-
-void Cimgcap(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
 {
     grab_frame();   // capture frame for processing
 }
 
-void Cimgrcap(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
+void Cvrcap(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)
+void Cvdiff(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
 {
     frame_diff_flag = Param[0]->Val->Integer;   // set/clear frame_diff_flag
 }
 
+void Cvpix(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
+{
+    int x, y, ix;
+    x = Param[0]->Val->Integer;
+    y = Param[1]->Val->Integer;
+    ix = vpix((unsigned char *)FRAME_BUF, x, y);
+    Iy1 = ((ix>>16) & 0x000000FF);  // Y1
+    Iu1 = ((ix>>24) & 0x000000FF);  // U
+    Iv1 = ((ix>>8)  & 0x000000FF);  // V
+}
+
+void Cvmean(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) 
+{
+    vmean((unsigned char *)FRAME_BUF);
+    Iy1 = mean[0];
+    Iu1 = mean[1];
+    Iv1 = mean[2];
+}
+
+//    search for blob by color, index;  return center point X,Y and width Z
+void Cvblob(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)    {
+    int ix, iblob, numblob;
+
+    ix = Param[0]->Val->Integer;
+    if (ix > MAX_COLORS)
+        ProgramFail(NULL, "blob():  invalid color index");
+    iblob = Param[1]->Val->Integer;
+    if (iblob > MAX_BLOBS)
+        ProgramFail(NULL, "blob():  invalid blob index");
+        
+    numblob = vblob((unsigned char *)FRAME_BUF, (unsigned char *)FRAME_BUF3, ix);
+
+    if (blobcnt[iblob] == 0) {
+        Blobcnt = 0;
+    } else {
+        Blobcnt = blobcnt[iblob];
+        Blobx1 = blobx1[iblob];
+        Blobx2 = blobx2[iblob];
+        Bloby1 = bloby1[iblob];
+        Bloby2 = bloby2[iblob];
+    }
+    ReturnValue->Val->Integer = numblob;
+}
+
 void Ccompass(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)  // return reading from HMC6352 I2C compass
 {
     unsigned char i2c_data[2];
@@ -237,31 +286,6 @@ void Cwritei2c(struct ParseState *Parser, struct Value *ReturnValue, struct Valu
     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, numblob;
-
-    ix = Param[0]->Val->Integer;
-    if (ix > MAX_COLORS)
-        ProgramFail(NULL, "blob():  bad parameter");
-    iblob = Param[1]->Val->Integer;
-    if (iblob > MAX_BLOBS)
-        ProgramFail(NULL, "blob():  bad parameter");
-        
-    numblob = vblob((unsigned char *)FRAME_BUF, (unsigned char *)FRAME_BUF3, ix);
-
-    if (blobcnt[iblob] == 0) {
-        Blobcnt = 0;
-    } else {
-        Blobcnt = blobcnt[iblob];
-        Blobx1 = blobx1[iblob];
-        Blobx2 = blobx2[iblob];
-        Bloby1 = bloby1[iblob];
-        Bloby2 = bloby2[iblob];
-    }
-    ReturnValue->Val->Integer = numblob;
-}
-
 static int cosine[] = {
 10000, 9998, 9994, 9986, 9976, 9962, 9945, 9925, 9903, 9877, 
  9848, 9816, 9781, 9744, 9703, 9659, 9613, 9563, 9511, 9455, 
@@ -322,6 +346,65 @@ void Ctan(struct ParseState *Parser, struct Value *ReturnValue, struct Value **P
     if (ix < 360)  { ReturnValue->Val->Integer = -(100 * cosine[ix-270]) / cosine[360-ix];  return; }
 }
 
+void Casin(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)  // asin(y,hyp)
+{
+    int y, hyp, quot, sgn, ix;
+	y = Param[0]->Val->Integer;
+	hyp = Param[1]->Val->Integer;
+    if (y > hyp)
+        ProgramFail(NULL, "asin():  opposite greater than hypotenuse");
+	if (y == 0) {
+        ReturnValue->Val->Integer = 0;
+	    return;
+	}
+	sgn = hyp * y;
+	if (hyp < 0) 
+	    hyp = -hyp;
+	if (y < 0)
+	    y = -y;
+	quot = (y * 10000) / hyp;
+	if (quot > 9999)
+	    quot = 9999;
+	for (ix=0; ix<90; ix++)
+	    if ((quot < cosine[ix]) && (quot >= cosine[ix+1]))
+	        break;
+	if (sgn < 0)
+	    ReturnValue->Val->Integer = -(90-ix);
+	else
+	    ReturnValue->Val->Integer = 90-ix;
+}
+
+void Cacos(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)  // acos(x,hyp)
+{
+    int x, hyp, quot, sgn, ix;
+	x = Param[0]->Val->Integer;
+	hyp = Param[1]->Val->Integer;
+    if (x > hyp)
+        ProgramFail(NULL, "acos():  adjacent greater than hypotenuse");
+	if (x == 0) {
+	    if (hyp < 0)
+            ReturnValue->Val->Integer = -90;
+        else
+            ReturnValue->Val->Integer = 90;
+	    return;
+	}
+	sgn = hyp * x;
+	if (hyp < 0) 
+	    hyp = -hyp;
+	if (x < 0)
+	    x = -x;
+	quot = (x * 10000) / hyp;
+	if (quot > 9999)
+	    quot = 9999;
+	for (ix=0; ix<90; ix++)
+	    if ((quot < cosine[ix]) && (quot >= cosine[ix+1]))
+	        break;
+	if (sgn < 0)
+	    ReturnValue->Val->Integer = -ix;
+	else
+	    ReturnValue->Val->Integer = ix;
+}
+
 void Catan(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)  // atan(y,x)
 {
     int x,y, angle, coeff_1, coeff_2, r;
@@ -363,16 +446,18 @@ struct LibraryFunction PlatformLibrary[] =
     { Crand,        "int rand(int)" },
     { Ctime,        "int time()" },
     { Cmotors,      "void motors(int, int)" },
-    { Cservo,       "void servo(int, int)" },
+    { Cservos,      "void servos(int, int)" },
-    { Cservo2,      "void servo2(int, int)" },
+    { Cservos2,     "void servos2(int, int)" },
     { Claser,       "void laser(int)" },
     { Csonar,       "void sonar(int)" },
     { Crange,       "int range()" },
-    { Ccolor,       "void color(int, int, int, int, int, int, int)" },
+    { Cvcolor,      "void vcolor(int, int, int, int, int, int, int)" },
-    { Cblob,         "int blob(int, int)" },
+    { Cvcap,        "void vcap()" },
-    { Cimgcap,      "void imgcap()" },
+    { Cvrcap,       "void vrcap()" },
-    { Cimgrcap,     "void imgrcap()" },
+    { Cvdiff,       "void vdiff(int)" },
-    { Cimgdiff,     "void imgdiff(int)" },
+    { Cvpix,        "void vpix(int, int)" },
+    { Cvmean,       "void vmean()" },
+    { Cvblob,       "int vblob(int, int)" },
     { Ccompass,     "int compass(int)" },
     { Creadi2c,     "int readi2c(int, int)" },
     { Creadi2c2,    "int readi2c2(int, int)" },
@@ -380,6 +465,8 @@ struct LibraryFunction PlatformLibrary[] =
     { Csin,         "int sin(int)" },
     { Ccos,         "int cos(int)" },
     { Ctan,         "int tan(int)" },
+    { Casin,        "int asin(int, int)" },
+    { Cacos,        "int acos(int, int)" },
     { Catan,        "int atan(int, int)" },
     { NULL,         NULL }
 };