I observed similar issues as the ones you described. However, since I was successful running the button test program, I trued using ButtonWaitForAnyPress (used in the button test program) instead of checkButtons. This time the program worked as expected. The "sleep" function was there in the initial program to introduce one second delays between readings. You could comment it out if you need more frequent readings, alternatively you can use usleep (micro second sleep) to specify delays shorter than one second.
Regarding your concerns about John's functions not being compatible with the Lego “lms2012.h†include file. That should not be a problem, It is always possible to redefine some constants as needed (similar to what I did). That solution is even available with commercial (closed source) libraries.
Here is the code I tested:
Code: Select all
#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include "lms2012.h"
#include "ev3_button.h"
#include "ev3_lcd.h"
//Runtime constants
const int MAX_SAMPLES = 1000;
//Global variables and constants used by the sensor handling functions
#define TITLE_DELAY 1000
const int XGL_PACKET_SIZE = 10; //2(gyyro angle) + 2(gyro rate) + 2(acc x) + 2(acc y) + 2(acc z)
const char XGL_PORT = 0x0; //The ports are designated as XGL_PORT_NUMBER-1
int xgl_device_file;
IIC *pXgl;
int init_xg1300l_gyro()
{
IICDAT IicDat;
char buf[120];
//Open the device xgl_device_file
if((xgl_device_file = open(IIC_DEVICE_NAME, O_RDWR | O_SYNC)) == -1) {
sprintf(buf, "Failed to open device");
LcdText(1, 0, 60, buf);
return 0;
}
pXgl = (IIC*)mmap(0, sizeof(IIC), PROT_READ | PROT_WRITE, MAP_FILE | MAP_SHARED, xgl_device_file, 0);
if (pXgl == MAP_FAILED) {
sprintf(buf, "Map failed");
LcdText(1, 0, 60, buf);
return 0;
}
//Setup IIC to read 2 packets
IicDat.Port = XGL_PORT;
IicDat.Time = 0;
IicDat.Repeat = 0;
IicDat.RdLng = XGL_PACKET_SIZE;
IicDat.WrLng = 2;
// Set the device I2C address
IicDat.WrData[0] = 0x01;
// Specify the register that will be read (0x42 = angle)
IicDat.WrData[1] = 0x42;
// Setup I2C comunication
ioctl(xgl_device_file,IIC_SETUP,&IicDat);
sprintf(buf,"Device is ready");
LcdText(1, 0, 60, buf);
return 1;
}
void get_xg1300l_gyro(float *angle, float *rate, float *acc_x, float *acc_y, float *acc_z)
{
//Compute angle, angular rate and accelerations
*acc_z = (pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][0]*256+pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][1])/100.0;
*acc_y = (pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][2]*256+pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][3])/100.0;
*acc_x = (pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][4]*256+pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][5])/100.0;
*rate = (pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][6]*256+pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][7])/100.0;
*angle = (pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][8]*256+pXgl->Raw[XGL_PORT][pXgl->Actual[XGL_PORT]][9])/100.0;
}
void close_xg1300l_gyro() //Close the device xgl_device_file
{
char buf[120];
sprintf(buf,"Closing device\n");
LcdText(1, 0, 60, buf);
close(xgl_device_file);
}
int main()
{
char buf[120];
float angle;
float rate;
float acc_x;
float acc_y;
float acc_z;
ButtonLedInit();
LcdInit();
LcdClean();
if(!init_xg1300l_gyro())
return -1;
LcdClean();
while(1)
{
get_xg1300l_gyro(&angle, &rate, &acc_x, &acc_y, &acc_z);
sprintf(buf,"Angle = %0.2f [deg]", angle);
LcdText(1, 0, 10, buf);
sprintf(buf,"Rate = %0.2f [deg/sec]", rate);
LcdText(1, 0, 30, buf);
sprintf(buf,"AccX = %0.2f [g]", acc_x);
LcdText(1, 0, 50, buf);
sprintf(buf,"AccY = %0.2f [g]", acc_y);
LcdText(1, 0, 70, buf);
sprintf(buf,"AccZ = %0.2f [g]", acc_z);
LcdText(1, 0, 90, buf);
if(ButtonWaitForAnyPress(TITLE_DELAY) == BUTTON_ID_ESCAPE)
break;
sleep(1);
}
close_xg1300l_gyro();
LcdExit();
ButtonLedExit();
return 1;
}