From 414ae8a3c2e5834c82c72cb2ec70091c079d448a Mon Sep 17 00:00:00 2001 From: "surveyor.com" Date: Wed, 8 Apr 2009 01:17:12 +0000 Subject: [PATCH] added peek(), poke(), battery(), iodir(), ioread() and iowrite() to Surveyor robot functions full function list is now found at http://www.surveyor.com/C.html git-svn-id: http://picoc.googlecode.com/svn/trunk@220 21eae674-98b7-11dd-bd71-f92a316d2d60 --- library_surveyor.c | 113 ++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 112 insertions(+), 1 deletion(-) diff --git a/library_surveyor.c b/library_surveyor.c index 600133b..dc07cba 100644 --- a/library_surveyor.c +++ b/library_surveyor.c @@ -59,6 +59,86 @@ void Ctime(struct ParseState *Parser, struct Value *ReturnValue, struct Value ** ReturnValue->Val->Integer = (int)readRTC(); } +void Ciodir(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) +{ + int dir; + + dir = Param[0]->Val->Integer; + *pPORTHIO_DIR |= ((dir << 26) & 0xFC00); // H15/14/13/12/11/10 - 1=output, 0=input + *pPORTHIO_INEN |= ((~dir << 26) & 0xFC00); // invert dir bits to enable inputs +} + +void Cioread(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) +{ + ReturnValue->Val->Integer = (*pPORTHIO >> 26) & 0x003F; +} + +void Ciowrite(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) +{ + *pPORTHIO = ((Param[0]->Val->Integer << 26) & 0xFC00) + (*pPORTHIO & 0x03FF); +} + +void Cpeek(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) +{ + int size, ptr; + unsigned char *cp; + unsigned short *sp; + unsigned int *ip; + + /* x = peek(addr, size); + mask ptr to align with word size */ + ptr = Param[0]->Val->Integer; + size = Param[1]->Val->Integer; + switch (size) { + case 1: // char * + cp = (unsigned char *)ptr; + ReturnValue->Val->Integer = (int)((unsigned int)*cp); + break; + case 2: // short * + sp = (unsigned short *)(ptr & 0xFFFFFFFE); // align with even boundary + ReturnValue->Val->Integer = (int)((unsigned short)*sp); + break; + case 4: // int * + ip = (unsigned int *)(ptr & 0xFFFFFFFC); // aling with quad boundary + ReturnValue->Val->Integer = (int)*ip; + break; + default: + ReturnValue->Val->Integer = 0; + break; + } +} + +void Cpoke(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) +{ + int size, ptr, val; + unsigned char *cp; + unsigned short *sp; + unsigned int *ip; + + /* x = poke(addr, size, val); + mask ptr to align with word size */ + ptr = Param[0]->Val->Integer; + size = Param[1]->Val->Integer; + val = Param[2]->Val->Integer; + switch (size) { + case 1: // char * + cp = (unsigned char *)ptr; + printf("cp = 0x%x\n\r", (int)cp); + *cp = (unsigned char)(val & 0x000000FF); + break; + case 2: // short * + sp = (unsigned short *)(ptr & 0xFFFFFFFE); + printf("sp = 0x%x\n\r", (int)sp); + *sp = (unsigned short)(val & 0x0000FFFF); + break; + case 4: // int * + ip = (unsigned int *)(ptr & 0xFFFFFFFC); + printf("ip = 0x%x\n\r", (int)ip); + *ip = val; + break; + } +} + void Cmotors(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) { lspeed = Param[0]->Val->Integer; @@ -76,6 +156,23 @@ void Cmotors(struct ParseState *Parser, struct Value *ReturnValue, struct Value setPWM(lspeed, rspeed); } +void Cmotors2(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) +{ + lspeed2 = Param[0]->Val->Integer; + if ((lspeed2 < -100) || (lspeed2 > 100)) + ProgramFail(NULL, "motors2(): left motor value out of range"); + rspeed2 = Param[1]->Val->Integer; + if ((rspeed2 < -100) || (rspeed2 > 100)) + ProgramFail(NULL, "motors2(): right motor value out of range"); + if (!pwm2_init) { + initPWM2(); + pwm2_init = 1; + pwm2_mode = PWM_PWM; + base_speed2 = 50; + } + setPWM2(lspeed2, rspeed2); +} + void Cservos(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) { int lspeed, rspeed; @@ -144,6 +241,14 @@ void Crange(struct ParseState *Parser, struct Value *ReturnValue, struct Value * ReturnValue->Val->Integer = laser_range(0); } +void Cbattery(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) +{ + if (*pPORTHIO & 0x0004) + ReturnValue->Val->Integer = 0; // low battery voltage detected + else + ReturnValue->Val->Integer = 1; // battery voltage okay +} + void Cvcolor(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // set color bin - // vcolor (color, ymin, ymax, umin, umax, vmin, vmax); { @@ -523,12 +628,18 @@ struct LibraryFunction PlatformLibrary[] = { Cdelay, "void delay(int)" }, { Crand, "int rand(int)" }, { Ctime, "int time()" }, + { Ciodir, "void iodir(int)" }, + { Cioread, "int ioread()" }, + { Ciowrite, "void iowrite(int)" }, + { Cpeek, "int peek(int, int)" }, + { Cpoke, "void poke(int, int, int)" }, { Cmotors, "void motors(int, int)" }, { Cservos, "void servos(int, int)" }, { Cservos2, "void servos2(int, int)" }, { Claser, "void laser(int)" }, - { Csonar, "void sonar(int)" }, + { Csonar, "int sonar(int)" }, { Crange, "int range()" }, + { Cbattery, "int battery()" }, { Cvcolor, "void vcolor(int, int, int, int, int, int, int)" }, { Cvcap, "void vcap()" }, { Cvrcap, "void vrcap()" },