ea10e1a369
git-svn-id: http://picoc.googlecode.com/svn/trunk@410 21eae674-98b7-11dd-bd71-f92a316d2d60
799 lines
28 KiB
C
799 lines
28 KiB
C
#include "picoc.h"
|
|
|
|
static int Blobcnt, Blobx1, Blobx2, Bloby1, Bloby2, Iy1, Iy2, Iu1, Iu2, Iv1, Iv2;
|
|
static int GPSlat, GPSlon, GPSalt, GPSfix, GPSsat, GPSutc, Elcount, Ercount;
|
|
static int ScanVect[16], NNVect[NUM_OUTPUT];
|
|
|
|
void PlatformLibraryInit()
|
|
{
|
|
struct ValueType *IntArrayType;
|
|
|
|
IntArrayType = TypeGetMatching(NULL, &IntType, TypeArray, 16, NULL);
|
|
VariableDefinePlatformVar(NULL, "scanvect", IntArrayType, (union AnyValue *)&ScanVect, FALSE);
|
|
VariableDefinePlatformVar(NULL, "neuron", IntArrayType, (union AnyValue *)&NNVect, FALSE);
|
|
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);
|
|
VariableDefinePlatformVar(NULL, "lcount", &IntType, (union AnyValue *)&Elcount, FALSE);
|
|
VariableDefinePlatformVar(NULL, "rcount", &IntType, (union AnyValue *)&Ercount, 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);
|
|
VariableDefinePlatformVar(NULL, "gpslat", &IntType, (union AnyValue *)&GPSlat, FALSE);
|
|
VariableDefinePlatformVar(NULL, "gpslon", &IntType, (union AnyValue *)&GPSlon, FALSE);
|
|
VariableDefinePlatformVar(NULL, "gpsalt", &IntType, (union AnyValue *)&GPSalt, FALSE);
|
|
VariableDefinePlatformVar(NULL, "gpsfix", &IntType, (union AnyValue *)&GPSfix, FALSE);
|
|
VariableDefinePlatformVar(NULL, "gpssat", &IntType, (union AnyValue *)&GPSsat, FALSE);
|
|
VariableDefinePlatformVar(NULL, "gpsutc", &IntType, (union AnyValue *)&GPSutc, FALSE);
|
|
}
|
|
|
|
void Csignal(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // check for kbhit, return t or nil
|
|
{
|
|
if (getsignal())
|
|
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();
|
|
}
|
|
|
|
void Cdelay(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
|
|
{
|
|
int del;
|
|
|
|
del = Param[0]->Val->Integer;
|
|
if ((del < 0) || (del > 1000000))
|
|
return;
|
|
delayMS(del);
|
|
}
|
|
|
|
void Crand(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
|
|
{
|
|
ReturnValue->Val->Integer = (int)rand() % (unsigned int)(Param[0]->Val->Integer + 1);
|
|
}
|
|
|
|
void Ctime(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
|
|
{
|
|
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 << 10) & 0xFC00) + (*pPORTHIO_DIR & 0x03FF); // H15/14/13/12/11/10 - 1=output, 0=input
|
|
*pPORTHIO_INEN = (((~dir) << 10) & 0xFC00) + (*pPORTHIO_INEN & 0x03FF); // invert dir bits to enable inputs
|
|
}
|
|
|
|
void Cioread(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
|
|
{
|
|
ReturnValue->Val->Integer = (*pPORTHIO >> 10) & 0x003F;
|
|
}
|
|
|
|
void Ciowrite(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
|
|
{
|
|
*pPORTHIO = ((Param[0]->Val->Integer << 10) & 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;
|
|
*cp = (unsigned char)(val & 0x000000FF);
|
|
break;
|
|
case 2: // short *
|
|
sp = (unsigned short *)(ptr & 0xFFFFFFFE);
|
|
*sp = (unsigned short)(val & 0x0000FFFF);
|
|
break;
|
|
case 4: // int *
|
|
ip = (unsigned int *)(ptr & 0xFFFFFFFC);
|
|
*ip = val;
|
|
break;
|
|
default: // don't bother with bad value
|
|
break;
|
|
}
|
|
}
|
|
|
|
void Cencoders(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
|
|
{
|
|
unsigned int ix;
|
|
|
|
ix = encoders(); // read left and right encoders; save data to C globals lcount, rcount
|
|
Elcount = (ix >> 16) & 0x0000FFFF;
|
|
Ercount = ix & 0x0000FFFF;
|
|
}
|
|
|
|
void Cmotors(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
|
|
{
|
|
lspeed = Param[0]->Val->Integer;
|
|
if ((lspeed < -100) || (lspeed > 100))
|
|
ProgramFail(NULL, "motors(): left motor value out of range");
|
|
rspeed = Param[1]->Val->Integer;
|
|
if ((rspeed < -100) || (rspeed > 100))
|
|
ProgramFail(NULL, "motors(): right motor value out of range");
|
|
if (!pwm1_init) {
|
|
initPWM();
|
|
pwm1_init = 1;
|
|
pwm1_mode = PWM_PWM;
|
|
base_speed = 50;
|
|
}
|
|
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;
|
|
|
|
lspeed = Param[0]->Val->Integer;
|
|
if ((lspeed < 0) || (lspeed > 100))
|
|
ProgramFail(NULL, "servos(): TMR2 value out of range");
|
|
rspeed = Param[1]->Val->Integer;
|
|
if ((rspeed < 0) || (rspeed > 100))
|
|
ProgramFail(NULL, "servos()(): TMR3 value out of range");
|
|
if (!pwm1_init) {
|
|
initPPM1();
|
|
pwm1_init = 1;
|
|
pwm1_mode = PWM_PPM;
|
|
}
|
|
setPPM1(lspeed, rspeed);
|
|
}
|
|
|
|
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, "servos2(): TMR6 value out of range");
|
|
rspeed = Param[1]->Val->Integer;
|
|
if ((rspeed < 0) || (rspeed > 100))
|
|
ProgramFail(NULL, "servos2(): TMR7 value out of range");
|
|
if (!pwm2_init) {
|
|
initPPM2();
|
|
pwm2_init = 1;
|
|
pwm2_mode = PWM_PPM;
|
|
}
|
|
setPPM2(lspeed, rspeed);
|
|
}
|
|
|
|
void Claser(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // laser(1) turns them on, laser(0) turns them off
|
|
{
|
|
*pPORTHIO &= 0xFD7F; // turn off both lasers
|
|
switch (Param[0]->Val->Integer) {
|
|
case 1:
|
|
*pPORTHIO |= 0x0080; // turn on left laser
|
|
break;
|
|
case 2:
|
|
*pPORTHIO |= 0x0200; // turn on right laser
|
|
break;
|
|
case 3:
|
|
*pPORTHIO |= 0x0280; // turn on both lasers
|
|
break;
|
|
}
|
|
}
|
|
|
|
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(): 1, 2, 3, 4 are only valid selections");
|
|
}
|
|
sonar();
|
|
ReturnValue->Val->Integer = sonar_data[i] / 100;
|
|
}
|
|
|
|
void Crange(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
|
|
{
|
|
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);
|
|
{
|
|
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 Cvcam(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // set camera functions -
|
|
// enable/disable AGC(4) / AWB(2) / AEC(1) camera controls
|
|
// vcam(7) = AGC+AWB+AEC on vcam(0) = AGC+AWB+AEC off
|
|
{
|
|
unsigned char cx, i2c_data[2];
|
|
|
|
cx = (unsigned char)Param[0]->Val->Integer & 0x07;
|
|
i2c_data[0] = 0x13;
|
|
i2c_data[1] = 0xC0 + cx;
|
|
i2cwrite(0x30, (unsigned char *)i2c_data, 1, SCCB_ON); // OV9655
|
|
i2cwrite(0x21, (unsigned char *)i2c_data, 1, SCCB_ON); // OV7725
|
|
}
|
|
|
|
void Cvfind(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // set color bin -
|
|
// vfind (color, x1, x2, y1, y2);
|
|
{
|
|
int ix, x1, x2, y1, y2;
|
|
|
|
ix = Param[0]->Val->Integer;
|
|
x1 = Param[1]->Val->Integer;
|
|
x2 = Param[2]->Val->Integer;
|
|
y1 = Param[3]->Val->Integer;
|
|
y2 = Param[4]->Val->Integer;
|
|
ReturnValue->Val->Integer = vfind((unsigned char *)FRAME_BUF, ix, x1, x2, y1, y2);
|
|
}
|
|
|
|
void Cvcap(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
|
|
{
|
|
grab_frame(); // capture frame for processing
|
|
}
|
|
|
|
void Cvrcap(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
|
|
{
|
|
grab_reference_frame(); // capture reference frame for differencing
|
|
}
|
|
|
|
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 Cvscan(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
|
|
{
|
|
int col, thresh, ix;
|
|
col = Param[0]->Val->Integer;
|
|
if ((col < 1) || (col > 9))
|
|
ProgramFail(NULL, "vscan(): number of columns must be between 1 and 9");
|
|
thresh = Param[1]->Val->Integer;
|
|
if ((thresh < 0) || (thresh > 9999))
|
|
ProgramFail(NULL, "vscan(): threshold must be between 0 and 9999");
|
|
ix = vscan((unsigned char *)SPI_BUFFER1, (unsigned char *)FRAME_BUF, thresh, (unsigned int)col, (unsigned int *)&ScanVect[0]);
|
|
ReturnValue->Val->Integer = ix;
|
|
}
|
|
|
|
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) || (numblob == -1)) {
|
|
Blobcnt = 0;
|
|
} else {
|
|
Blobcnt = blobcnt[iblob];
|
|
Blobx1 = blobx1[iblob];
|
|
Blobx2 = blobx2[iblob];
|
|
Bloby1 = bloby1[iblob];
|
|
Bloby2 = bloby2[iblob];
|
|
}
|
|
ReturnValue->Val->Integer = numblob;
|
|
}
|
|
|
|
void Cvjpeg (struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) {
|
|
unsigned int image_size, qual;
|
|
unsigned char *output_start, *output_end;
|
|
|
|
qual = Param[0]->Val->Integer;
|
|
if ((qual < 1) || (qual > 8))
|
|
ProgramFail(NULL, "vjpeg(): quality parameter out of range");
|
|
|
|
output_start = (unsigned char *)JPEG_BUF;
|
|
output_end = encode_image((unsigned char *)FRAME_BUF, output_start, qual,
|
|
FOUR_TWO_TWO, imgWidth, imgHeight);
|
|
image_size = (unsigned int)(output_end - output_start);
|
|
|
|
ReturnValue->Val->Integer = image_size;
|
|
}
|
|
|
|
void Cvsend (struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) {
|
|
unsigned int ix, image_size;
|
|
unsigned char *cp;
|
|
|
|
image_size = Param[0]->Val->Integer;
|
|
if ((image_size < 0) || (image_size > 200000))
|
|
ProgramFail(NULL, "vsend(): image size out of range");
|
|
|
|
led1_on();
|
|
|
|
cp = (unsigned char *)JPEG_BUF;
|
|
for (ix=0; ix<image_size; ix++)
|
|
putchar(*cp++);
|
|
|
|
led0_on();
|
|
}
|
|
|
|
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(0x22, (unsigned char *)i2c_data, 2, SCCB_ON);
|
|
delayMS(20);
|
|
i2c_data[0] = 0x41;
|
|
i2cread(0x22, (unsigned char *)i2c_data, 2, SCCB_ON);
|
|
ix = ((unsigned int)(i2c_data[0] << 8) + i2c_data[1]) / 10;
|
|
ReturnValue->Val->Integer = ix;
|
|
}
|
|
|
|
void Ctilt(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // return reading from HMC6352 I2C compass
|
|
{
|
|
unsigned int ix;
|
|
|
|
ix = (unsigned int)Param[0]->Val->Integer;
|
|
if ((ix<1) || (ix>3))
|
|
ProgramFail(NULL, "tilt(): invalid channel");
|
|
ReturnValue->Val->Integer = tilt(ix);
|
|
}
|
|
|
|
void Canalog(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // return reading from HMC6352 I2C compass
|
|
{
|
|
unsigned char i2c_data[3], device_id;
|
|
unsigned int ix, channel;
|
|
unsigned char mask1[] = { 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x04, 0x08 };
|
|
unsigned char mask2[] = { 0x10, 0x20, 0x40, 0x80, 0x00, 0x00, 0x00, 0x00 };
|
|
|
|
// decide which i2c device based on channel range
|
|
ix = (unsigned char)Param[0]->Val->Integer;
|
|
if ((ix<1) || (ix>28))
|
|
ProgramFail(NULL, "analog(): invalid channel");
|
|
device_id = 0;
|
|
switch (ix / 10) {
|
|
case 0:
|
|
device_id = 0x20; // channels 1-8
|
|
break;
|
|
case 1:
|
|
device_id = 0x23; // channels 11-18
|
|
break;
|
|
case 2:
|
|
device_id = 0x24; // channels 21-28
|
|
break;
|
|
}
|
|
channel = ix % 10;
|
|
if ((channel<1) || (channel>8))
|
|
ProgramFail(NULL, "analog(): invalid channel");
|
|
|
|
// set timer register 3
|
|
i2c_data[0] = 0x03;
|
|
i2c_data[1] = 0x01;
|
|
i2cwrite(device_id, (unsigned char *)i2c_data, 1, SCCB_ON);
|
|
|
|
// set analog channel
|
|
i2c_data[0] = 0x02;
|
|
i2c_data[1] = mask1[channel-1];
|
|
i2c_data[2] = mask2[channel-1];
|
|
i2cwritex(device_id, (unsigned char *)i2c_data, 3, SCCB_ON);
|
|
|
|
// small delay
|
|
delayUS(1000);
|
|
|
|
// read data
|
|
i2c_data[0] = 0x00;
|
|
i2cread(device_id, (unsigned char *)i2c_data, 2, SCCB_ON);
|
|
ix = (((i2c_data[0] & 0x0F) << 8) + i2c_data[1]);
|
|
ReturnValue->Val->Integer = ix;
|
|
}
|
|
|
|
void Cgps(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
|
|
{
|
|
gps_parse();
|
|
GPSlat = gps_gga.lat;
|
|
GPSlon = gps_gga.lon;
|
|
GPSalt = gps_gga.alt;
|
|
GPSfix = gps_gga.fix;
|
|
GPSsat = gps_gga.sat;
|
|
GPSutc = gps_gga.utc;
|
|
}
|
|
|
|
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 Csin(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // sin(angle)
|
|
{
|
|
int ix;
|
|
|
|
ix = Param[0]->Val->Integer; // input to function is angle in degrees
|
|
ReturnValue->Val->Integer = sin(ix);
|
|
}
|
|
|
|
void Ccos(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // cos(angle)
|
|
{
|
|
int ix;
|
|
|
|
ix = Param[0]->Val->Integer; // input to function is angle in degrees
|
|
ReturnValue->Val->Integer = cos(ix);
|
|
}
|
|
|
|
void Ctan(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // tan(angle)
|
|
{
|
|
int ix;
|
|
|
|
ix = Param[0]->Val->Integer; // input to function is angle in degrees
|
|
ReturnValue->Val->Integer = tan(ix);
|
|
}
|
|
|
|
void Casin(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // asin(y,hyp)
|
|
{
|
|
int y, hyp;
|
|
y = Param[0]->Val->Integer;
|
|
hyp = Param[1]->Val->Integer;
|
|
if (y > hyp)
|
|
ProgramFail(NULL, "asin(): opposite greater than hypotenuse");
|
|
ReturnValue->Val->Integer = asin(y, hyp);
|
|
}
|
|
|
|
void Cacos(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // acos(x,hyp)
|
|
{
|
|
int x, hyp;
|
|
x = Param[0]->Val->Integer;
|
|
hyp = Param[1]->Val->Integer;
|
|
if (x > hyp)
|
|
ProgramFail(NULL, "acos(): adjacent greater than hypotenuse");
|
|
ReturnValue->Val->Integer = acos(x, hyp);
|
|
}
|
|
|
|
void Catan(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // atan(y,x)
|
|
{
|
|
int x ,y;
|
|
y = Param[0]->Val->Integer;
|
|
x = Param[1]->Val->Integer;
|
|
ReturnValue->Val->Integer = atan(y, x);
|
|
}
|
|
|
|
void Cgps_head(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // gps_head(lat1, lon1, lat2, lon2)
|
|
{
|
|
int lat1, lon1, lat2, lon2;
|
|
lat1 = Param[0]->Val->Integer;
|
|
lon1 = Param[1]->Val->Integer;
|
|
lat2 = Param[2]->Val->Integer;
|
|
lon2 = Param[3]->Val->Integer;
|
|
ReturnValue->Val->Integer = gps_head(lat1, lon1, lat2, lon2);
|
|
}
|
|
|
|
void Cgps_dist(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // gps_dist(lat1, lon1, lat2, lon2)
|
|
{
|
|
int lat1, lon1, lat2, lon2;
|
|
lat1 = Param[0]->Val->Integer;
|
|
lon1 = Param[1]->Val->Integer;
|
|
lat2 = Param[2]->Val->Integer;
|
|
lon2 = Param[3]->Val->Integer;
|
|
ReturnValue->Val->Integer = gps_dist(lat1, lon1, lat2, lon2);
|
|
}
|
|
|
|
void Csqrt(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) // sqrt(x)
|
|
{
|
|
int x;
|
|
x = Param[0]->Val->Integer;
|
|
ReturnValue->Val->Integer = isqrt(x);
|
|
}
|
|
|
|
void Cnnset(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) {
|
|
int ix, i1;
|
|
|
|
ix = Param[0]->Val->Integer;
|
|
if (ix > NUM_NPATTERNS)
|
|
ProgramFail(NULL, "nnset(): invalid index");
|
|
for (i1=0; i1<8; i1++)
|
|
npattern[ix*8 + i1] = (unsigned char)Param[i1+1]->Val->Integer;
|
|
}
|
|
|
|
void Cnnshow(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) {
|
|
int ix;
|
|
|
|
ix = Param[0]->Val->Integer;
|
|
if (ix > NUM_NPATTERNS)
|
|
ProgramFail(NULL, "nnshow(): invalid index");
|
|
nndisplay(ix);
|
|
}
|
|
|
|
void Cnninit(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) {
|
|
nninit_network();
|
|
}
|
|
|
|
void Cnntrain(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) {
|
|
int ix, i1;
|
|
|
|
nntrain_network(10000);
|
|
for (ix=0; ix<NUM_NPATTERNS; ix++) {
|
|
nnset_pattern(ix);
|
|
nncalculate_network();
|
|
for (i1=0; i1<NUM_OUTPUT; i1++)
|
|
printf(" %3d", N_OUT(i1)/10);
|
|
printf("\r\n");
|
|
}
|
|
}
|
|
|
|
void Cnntest(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) {
|
|
int ix, i1, i2, max;
|
|
unsigned char ch;
|
|
|
|
ix = 0;
|
|
for (i1=0; i1<8; i1++) {
|
|
ch = (unsigned char)Param[i1]->Val->Integer;
|
|
for (i2=0; i2<8; i2++) {
|
|
if (ch & nmask[i2])
|
|
N_IN(ix++) = 1024;
|
|
else
|
|
N_IN(ix++) = 0;
|
|
}
|
|
}
|
|
nncalculate_network();
|
|
ix = 0;
|
|
max = 0;
|
|
for (i1=0; i1<NUM_OUTPUT; i1++) {
|
|
NNVect[i1] = N_OUT(i1)/10;
|
|
if (max < NNVect[i1]) {
|
|
ix = i1;
|
|
max = NNVect[i1];
|
|
}
|
|
}
|
|
ReturnValue->Val->Integer = ix;
|
|
}
|
|
|
|
void Cnnmatchblob(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) {
|
|
int ix, i1, max;
|
|
|
|
ix = Param[0]->Val->Integer;
|
|
if (ix > MAX_BLOBS)
|
|
ProgramFail(NULL, "nnmatchblob(): invalid blob index");
|
|
if (!blobcnt[ix])
|
|
ProgramFail(NULL, "nnmatchblob(): not a valid blob");
|
|
/* use data still in blob_buf[] (FRAME_BUF3)
|
|
square the aspect ratio of x1, x2, y1, y2
|
|
then subsample blob pixels to populate N_IN(0:63) with 0:1024 values
|
|
then nncalculate_network() and display the N_OUT() results */
|
|
nnscale8x8((unsigned char *)FRAME_BUF3, blobix[ix], blobx1[ix], blobx2[ix],
|
|
bloby1[ix], bloby2[ix], imgWidth, imgHeight);
|
|
nncalculate_network();
|
|
ix = 0;
|
|
max = 0;
|
|
for (i1=0; i1<NUM_OUTPUT; i1++) {
|
|
NNVect[i1] = N_OUT(i1)/10;
|
|
if (max < NNVect[i1]) {
|
|
ix = i1;
|
|
max = NNVect[i1];
|
|
}
|
|
}
|
|
ReturnValue->Val->Integer = ix;
|
|
}
|
|
|
|
void Cnnlearnblob (struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) {
|
|
int ix;
|
|
|
|
ix = Param[0]->Val->Integer;
|
|
if (ix > NUM_NPATTERNS)
|
|
ProgramFail(NULL, "nnlearnblob(): invalid index");
|
|
if (!blobcnt[0])
|
|
ProgramFail(NULL, "nnlearnblob(): no blob to grab");
|
|
nnscale8x8((unsigned char *)FRAME_BUF3, blobix[0], blobx1[0], blobx2[0],
|
|
bloby1[0], bloby2[0], imgWidth, imgHeight);
|
|
nnpack8x8(ix);
|
|
nndisplay(ix);
|
|
}
|
|
|
|
void Cautorun (struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) {
|
|
int ix, t0;
|
|
unsigned char ch;
|
|
|
|
ix = Param[0]->Val->Integer;
|
|
t0 = readRTC();
|
|
while (readRTC() < (t0 + ix*1000)) { // watch for ESC in 'ix' seconds
|
|
if (getchar(&ch)) {
|
|
if (ch == 0x1B) { // if ESC found, exit picoC
|
|
printf("found ESC\r\n");
|
|
ExitBuf[40] = 1;
|
|
longjmp(ExitBuf, 1);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void Clineno (struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) {
|
|
ReturnValue->Val->Integer = Parser->Line;
|
|
}
|
|
|
|
void Cerrormsg (struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) {
|
|
PlatformErrorPrefix(Parser);
|
|
LibPrintf(Parser, ReturnValue, Param, NumArgs);
|
|
}
|
|
|
|
/* list of all library functions and their prototypes */
|
|
struct LibraryFunction PlatformLibrary[] =
|
|
{
|
|
{ Csignal, "int signal();" },
|
|
{ Cinput, "int input();" },
|
|
{ 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);" },
|
|
{ Cmotors2, "void motors2(int, int);" },
|
|
{ Cservos, "void servos(int, int);" },
|
|
{ Cservos2, "void servos2(int, int);" },
|
|
{ Cencoders, "void encoders();" },
|
|
{ Claser, "void laser(int);" },
|
|
{ Csonar, "int sonar(int);" },
|
|
{ Crange, "int range();" },
|
|
{ Cbattery, "int battery();" },
|
|
{ Cvcolor, "void vcolor(int, int, int, int, int, int, int);" },
|
|
{ Cvfind, "int vfind(int, int, int, int, int);" },
|
|
{ Cvcam, "void vcam(int);" },
|
|
{ Cvcap, "void vcap();" },
|
|
{ Cvrcap, "void vrcap();" },
|
|
{ Cvdiff, "void vdiff(int);" },
|
|
{ Cvpix, "void vpix(int, int);" },
|
|
{ Cvscan, "int vscan(int, int);" },
|
|
{ Cvmean, "void vmean();" },
|
|
{ Cvblob, "int vblob(int, int);" },
|
|
{ Cvjpeg, "int vjpeg(int);" },
|
|
{ Cvsend, "void vsend(int);" },
|
|
{ Ccompass, "int compass();" },
|
|
{ Canalog, "int analog(int);" },
|
|
{ Ctilt, "int tilt(int);" },
|
|
{ Cgps, "void gps();" },
|
|
{ Creadi2c, "int readi2c(int, int);" },
|
|
{ Creadi2c2, "int readi2c2(int, int);" },
|
|
{ Cwritei2c, "void writei2c(int, int, int);" },
|
|
{ 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);" },
|
|
{ Cgps_head, "int gps_head(int, int, int, int);" },
|
|
{ Cgps_dist, "int gps_dist(int, int, int, int);" },
|
|
{ Csqrt, "int sqrt(int);" },
|
|
{ Cnnshow, "void nnshow(int);" },
|
|
{ Cnnset, "void nnset(int, int, int, int, int, int, int, int, int);" },
|
|
{ Cnninit, "void nninit();" },
|
|
{ Cnntrain, "void nntrain();" },
|
|
{ Cnntest, "int nntest(int, int, int, int, int, int, int, int);" },
|
|
{ Cnnmatchblob, "int nnmatchblob(int);" },
|
|
{ Cnnlearnblob, "void nnlearnblob(int);" },
|
|
{ Cautorun, "void autorun(int);" },
|
|
{ Clineno, "int lineno();" },
|
|
{ Cerrormsg, "void errormsg(char *);" },
|
|
{ NULL, NULL }
|
|
};
|
|
|