From 6107592f37fe6ef30996801b274225b037713963 Mon Sep 17 00:00:00 2001 From: "surveyor.com" Date: Mon, 2 Mar 2009 02:58:03 +0000 Subject: [PATCH] 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 | 346 +++++++++++++++++++-------------------------- 1 file changed, 143 insertions(+), 203 deletions(-) diff --git a/platform_library.c b/platform_library.c index 1158948..542c9d9 100644 --- a/platform_library.c +++ b/platform_library.c @@ -13,17 +13,24 @@ 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; @@ -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(); } -/* -void imgcap() +extern int sonar_data[]; +void Csonar(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // read sonar module { - start_check(); - grab_frame(); - finish_check(); + unsigned int i; + i = Param[0]->Val->Integer; + 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(); - grab_reference_frame(); - finish_check(); + int ix; + + 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(); - eval_exp(&frame_diff_flag); - finish_check(); + 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 +} -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(); - eval_exp(&ix); - if ((ix < 0) || (ix > 9)) - sntx_err(PARAM_ERR); - - get_token(); // look for comma separator - if(*token != ',') - sntx_err(SYNTAX); - - eval_exp(&iblob); + ix = Param[0]->Val->Integer; + iblob = Param[1]->Val->Integer; if (iblob == 0) vblob((unsigned char *)FRAME_BUF, (unsigned char *)FRAME_BUF3, ix); if (blobcnt[iblob] == 0) { - assign_var("x", 0); - assign_var("y", 0); - assign_var("z", 0); + Blobcnt = 0; } else { - assign_var("x", (blobx1[iblob] + blobx2[iblob]) / 2); - assign_var("y", (bloby1[iblob] + bloby2[iblob]) / 2); - assign_var("z", (blobx2[iblob] - blobx1[iblob]) + 1); + Blobcnt = blobcnt[iblob]; + Blobx1 = blobx1[iblob]; + Blobx2 = blobx2[iblob]; + Bloby1 = bloby1[iblob]; + Bloby2 = bloby2[iblob]; } - finish_check(); - return blobcnt[iblob]; + ReturnValue->Val->Integer = 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