282 lines
6.6 KiB
C
282 lines
6.6 KiB
C
#include <stdio.h>
|
|
#include <io/move.h>
|
|
#include <sys/memory.h>
|
|
#include <ppu-types.h>
|
|
#include <io/camera.h>
|
|
#include <ppu-types.h>
|
|
#include <spurs/spurs.h>
|
|
#include <sys/spu.h>
|
|
#include <sys/thread.h>
|
|
#include <sys/systime.h>
|
|
|
|
#define SPURS_DEFAULT_PREFIX_NAME "gem_spurs"
|
|
|
|
u16 oldGemPad = 0;
|
|
u16 newGemPad = 0;
|
|
u16 newGemAnalogT = 0;
|
|
extern cameraReadInfo camread;
|
|
|
|
static inline float
|
|
vec_array(vec_float4 vec, unsigned int idx)
|
|
{
|
|
union {
|
|
vec_float4 vec;
|
|
float array[4];
|
|
} v;
|
|
|
|
v.vec = vec;
|
|
|
|
if (idx > 3)
|
|
return -1;
|
|
return v.array[idx];
|
|
}
|
|
|
|
int
|
|
initMove()
|
|
{
|
|
Spurs *spurs;
|
|
gemAttribute gem_attr;
|
|
u8 gem_spu_priorities[8] = { 1, 0, 0, 0, 0, 0, 0, 0 };
|
|
int ret;
|
|
int i;
|
|
|
|
spurs = initSpurs (NULL);
|
|
|
|
if (spurs == NULL)
|
|
goto error;
|
|
|
|
printf ("preparing GemAttribute structure with spurs\n");
|
|
|
|
initAttributeGem (&gem_attr, 1, NULL, spurs, gem_spu_priorities);
|
|
gem_attr->version = 2;
|
|
gem_attr->max = 1;
|
|
gem_attr->spurs = spurs;
|
|
gem_attr->memory = NULL;
|
|
gem_attr->spu_priorities[0] = 1;
|
|
for (i = 1; i < 8; ++i)
|
|
gem_attr->spu_priorities[i] = 0;
|
|
|
|
printf ("calling GemInit with GemAttribute structure version=%d max_connect=%d spurs=%X\n",
|
|
gem_attr.version, gem_attr.max, gem_attr.spurs);
|
|
ret = gemInit (&gem_attr);
|
|
printf ("return from GemInit %X \n", ret);
|
|
if (ret)
|
|
goto error;
|
|
|
|
ret = gemPrepareCamera (128, 0.5);
|
|
printf ("GemPrepareCamera return %d exposure set to 128 and quality to 0.5\n",
|
|
ret);
|
|
if (ret)
|
|
goto error;
|
|
ret = gemReset (0);
|
|
printf ("GemReset return %X \n", ret);
|
|
if (ret)
|
|
goto error;
|
|
return ret;
|
|
|
|
error:
|
|
if (spurs)
|
|
endSpurs (spurs);
|
|
return NULL;
|
|
}
|
|
|
|
int
|
|
endMove()
|
|
{
|
|
endSpurs (spurs);
|
|
gemEnd ();
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
proccessGem(int t)
|
|
{
|
|
int ret;
|
|
|
|
switch (t) {
|
|
case 0:
|
|
|
|
ret = gemUpdateStart (camread.buffer, camread.timestamp);
|
|
|
|
if (ret != 0)
|
|
{
|
|
printf ("Return from gemUpdateStart %X\n", ret);
|
|
}
|
|
break;
|
|
|
|
case 2:
|
|
|
|
ret = gemUpdateFinish ();
|
|
if (ret != 0)
|
|
{
|
|
printf ("Return from gemUpdateFinish %X\n", ret);
|
|
}
|
|
break;
|
|
|
|
case 3:
|
|
ret = gemConvertVideoFinish ();
|
|
if (ret != 0)
|
|
{
|
|
printf ("Return from gemConvertVideoFinish %X\n", ret);
|
|
}
|
|
break;
|
|
|
|
default:
|
|
ret = -1;
|
|
break;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
void
|
|
readGemPad(int num_gem)
|
|
{
|
|
gemState gem_state;
|
|
int ret;
|
|
unsigned int hues[] = { 4 << 24, 4 << 24, 4 << 24, 4 << 24 };
|
|
ret = gemGetState (0, 0, -22000, &gem_state);
|
|
|
|
newGemPad = gem_state.paddata.buttons & (~oldGemPad);
|
|
newGemAnalogT = gem_state.paddata.ANA_T;
|
|
oldGemPad = gem_state.paddata.buttons;
|
|
|
|
switch (ret) {
|
|
case 2:
|
|
gemForceRGB (num_gem, 0.5, 0.5, 0.5);
|
|
break;
|
|
|
|
case 5:
|
|
|
|
gemTrackHues (hues, NULL);
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void
|
|
readGemAccPosition(int num_gem)
|
|
{
|
|
vec_float4 position;
|
|
|
|
gemGetAccelerometerPositionInDevice (num_gem, &position);
|
|
|
|
printf (" accelerometer device coordinates [%f,%f,%f,%f]\n",
|
|
vec_array (position, 0), vec_array (position, 1), vec_array (position, 2),
|
|
vec_array (position, 3));
|
|
}
|
|
|
|
void
|
|
readGemInertial(int num_gem)
|
|
{
|
|
gemInertialState gem_inertial_state;
|
|
int ret;
|
|
|
|
ret = gemGetInertialState (num_gem, 0, -22000, &gem_inertial_state);
|
|
printf ("gemGetInertialState return %X\n", ret);
|
|
printf ("counter %d temperature %f\n", gem_inertial_state.counter,
|
|
gem_inertial_state.temperature);
|
|
|
|
printf (" accelerometer sensor [%f,%f,%f,%f]\n",
|
|
vec_array (gem_inertial_state.accelerometer, 0),
|
|
vec_array (gem_inertial_state.accelerometer, 1),
|
|
vec_array (gem_inertial_state.accelerometer, 2),
|
|
vec_array (gem_inertial_state.accelerometer, 3));
|
|
|
|
printf (" accelerometer_bias sensor [%f,%f,%f,%f]\n",
|
|
vec_array (gem_inertial_state.accelerometer_bias, 0),
|
|
vec_array (gem_inertial_state.accelerometer_bias, 1),
|
|
vec_array (gem_inertial_state.accelerometer_bias, 2),
|
|
vec_array (gem_inertial_state.accelerometer_bias, 3));
|
|
|
|
printf (" gyro sensor [%f,%f,%f,%f]\n", vec_array (gem_inertial_state.gyro,
|
|
0), vec_array (gem_inertial_state.gyro, 1),
|
|
vec_array (gem_inertial_state.gyro, 2),
|
|
vec_array (gem_inertial_state.gyro, 3));
|
|
|
|
printf (" gyro_bias sensor [%f,%f,%f,%f]\n",
|
|
vec_array (gem_inertial_state.gyro_bias, 0),
|
|
vec_array (gem_inertial_state.gyro_bias, 1),
|
|
vec_array (gem_inertial_state.gyro_bias, 2),
|
|
vec_array (gem_inertial_state.gyro_bias, 3));
|
|
}
|
|
|
|
void
|
|
readGem()
|
|
{
|
|
gemState gem_state;
|
|
proccessGem (0);
|
|
|
|
proccessGem (2);
|
|
|
|
readGemPad (0); // This will read buttons from Move
|
|
switch (newGemPad) {
|
|
case 1:
|
|
printf ("Select pressed \n");
|
|
break;
|
|
|
|
case 2:
|
|
printf ("T pressed value %d\n", newGemAnalogT);
|
|
printf
|
|
("Frame %d center of the sphere in world coordinates %f %f %f %f \n",
|
|
camread.frame, vec_array (gem_state.pos, 0), vec_array (gem_state.pos,
|
|
1), vec_array (gem_state.pos, 2), vec_array (gem_state.pos, 3));
|
|
break;
|
|
|
|
case 4:
|
|
printf ("Move pressed \n");
|
|
gemCalibrate (0);
|
|
break;
|
|
|
|
case 8:
|
|
printf ("Start pressed \n");
|
|
pos_x = 0;
|
|
pos_y = 0;
|
|
break;
|
|
|
|
case 16:
|
|
printf ("Triangle pressed \n");
|
|
getImageState ();
|
|
break;
|
|
|
|
case 32:
|
|
printf ("Circle pressed \n");
|
|
break;
|
|
|
|
case 64:
|
|
printf ("Cross pressed \n");
|
|
printf ("X,Y,Z position (mm) %f %f %f\n", vec_array (gem_state.pos, 0),
|
|
vec_array (gem_state.pos, 1), vec_array (gem_state.pos, 2));
|
|
readGemAccPosition (0);
|
|
break;
|
|
|
|
case 128:
|
|
printf ("Square pressed \n");
|
|
readGemInertial (0);
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void
|
|
getImageState()
|
|
{
|
|
int ret;
|
|
|
|
gemImageState imgState;
|
|
|
|
gemGetImageState (0, &imgState);
|
|
printf (" u [%f]\n", imgState.u);
|
|
printf (" v [%f]\n", imgState.v);
|
|
printf (" r [%f]\n", imgState.r);
|
|
printf (" projectionx [%f]\n", imgState.projectionx);
|
|
printf (" projectiony [%f]\n", imgState.projectiony);
|
|
printf (" distance [%f]\n", imgState.distance);
|
|
printf ("visible=%d r_valid=%d\n", imgState.visible, imgState.r_valid);
|
|
printf ("tiemestamp=%Ld\n", imgState.frame_time);
|
|
}
|
|
|