This code Assumes the IMUZero program has been used to calibrate the MPU6050 before use here.
Arduino Nano attached to a MPU 6050.
#include <I2Cdev.h>
#include "I2Cdev.h"
#include "MPU6050.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include <Wire.h>
#endif
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro2(0x69); / <-- use for AD0 high
////////////////////////////////////////////////////////////////////////
void setup() {
Serial.begin(115200);
// put your setup code here, to run once:
//while(!Serial.available());
Fastwire::setup(400, false);
accelgyro.initialize();
if(accelgyro.testConnection()){
}
else {
}
/*
Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -1131
Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -5595
Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 3855
Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 30
Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 44
Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 27
Serial.print("\n");*/
// accelgyro.setXAccelOffset(0);
// accelgyro.setYAccelOffset(0);
// accelgyro.setZAccelOffset(0);
accelgyro.setXGyroOffset(27);
accelgyro.setYGyroOffset(27);
accelgyro.setZGyroOffset(78);
accelgyro.setFullScaleGyroRange(0); //0 = +/- 250 degrees/sec | 1 = +/- 500 degrees/sec | 2 = +/- 1000 degrees/sec | 3 = +/- 2000 degrees/sec
accelgyro.setFullScaleAccelRange(0); //0 = +/- 2g | 1 = +/- 4g | 2 = +/- 8g | 3 = +/- 16g
delay(1000);
}
///////////////////////////////////////////////////////////////////////
#define M 20
#define O 20
float A2x=0.0,A2y=0.0,A2z=0.0;
float A3x=0.0,A3y=0.0,A3z=0.0;
float A1x=0.0,A1y=0.0,A1z=0.0;
int counter_m=0,counter_o=0;
bool calc_A3=false;
unsigned long dt=0;
double fdt=0.0;
int counter=0;
int N=1000;
int16_t ax, ay, az;
int16_t gx, gy, gz;
uint8_t BUFFER[14];
int state=0;
float angle1x=0.0;
float angle1y=0.0;
float angle1z=0.0;
float angle2x=0.0;
float angle2y=0.0;
float angle2z=0.0;
double fax=0.0,faz=0.0,fay=0.0,fd=0.0;
float theta;
#define DUTY_MAX 20
bool tripped=false;
bool flash=true;
unsigned int duty_counter=0,duty_cycleR=0,duty_cycleG=0,duty_cycleB=0;
///////////////////////////////////////////////////////////////////////////
void loop() {
// Serial.print(duty_counter%DUTY_MAX); Serial.print("\t");
// duty_counter++;
//////////////////////////////////////////////
fax=0.0;faz=0.0;fay=0.0;counter=0;
do{
tripped=false;
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);//
if(state>=0){
ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68, 0x3F, 2,BUFFER);
if(state>=0){
az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL2");
Fastwire::reset();
tripped=true;
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_XOUT_H,2, BUFFER);//
if(state>=0){
ax= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
if(!tripped){
counter++;
fax+=ax;
fay+=ay;
faz+=az;
}
// Serial.print(ax); Serial.print("\t");
// Serial.print(ay); Serial.print("\t");
// Serial.print(az); Serial.print("\t");
// Serial.print( sqrt(faz*faz+fax*fax+fay*fay)); Serial.print("\t\t");
}while(counter<N);
fax=fax/float(N);
fay=fay/float(N);
faz=faz/float(N);
angle1y=180.0/M_PI*atan(fay/faz);
angle1x=180.0/M_PI*atan(-fax/sqrt(fay*fay+faz*faz));
////////////////////////////
// Serial.print(fax); Serial.print("\t");
// Serial.print(fay); Serial.print("\t");
// Serial.print(faz); Serial.print("\t");
// Serial.print(sqrt(fax*fax+fay*fay+faz*faz)-16356.0); Serial.println("");
////////////////////////////////////////////////////////////////////////////////////////////////
do{
A2x=0.0;A2y=0.0;A2z=0.0;
counter_o=0.0;
do{
A1x=0.0;A1y=0.0;A1z=0.0;
counter_m=0;fdt=0.0;
do{
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);//
if(state>=0){
ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68, 0x3F, 2,BUFFER);
if(state>=0){
az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL2");
Fastwire::reset();
tripped=true;
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_XOUT_H,2, BUFFER);//
if(state>=0){
ax= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_YOUT_H,2, BUFFER);//
if(state>=0){
gy= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_XOUT_H,2, BUFFER);//
if(state>=0){
gx= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_ZOUT_H,2, BUFFER);//
if(state>=0){
gz= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
if(!tripped){
if(dt!=0)fdt+=(micros()-dt)/1000000.0;
else fdt=0.0;
dt=micros();
counter_m++;
A1x+=gx;
A1y+=gy;
A1z+=gz;
}
}while(counter_m<M);
A1x/=1.0*M;
A1y/=1.0*M;
A1z/=1.0*M;
if(calc_A3 && (abs(A1x-A3x)>15.0 || abs(A1y-A3y)>15.0 || abs(A1z-A3z)>50.0 )){
angle1x+=fdt*A1x/131.0;
angle1y+=fdt*A1y/131.0;
angle1z+=fdt*A1z/131.0;
// Serial.print("*");
//counter_o=0;A2x=0.0;A2y=0.0;A2z=0.0;
}
else{
A2x+=A1x;
A2y+=A1y;
A2z+=A1z;
counter_o++;
}
/// Serial.print(abs(A1x-A3x)); Serial.print("\t");
// Serial.print(abs(A1y-A3y)); Serial.print("\t");
// Serial.print(abs(A1z-A3z));Serial.print("\t");
// Serial.print(A3x); Serial.print("\t");
// Serial.print(A3y); Serial.print("\t");
// Serial.print(A3z);Serial.print("\t");
Serial.print(angle1x); Serial.print("\t");
Serial.print(angle1y); Serial.print("\t");
Serial.print(angle1z);Serial.print("\n");
// Serial.print(fdt*1000);Serial.println("");
}while(counter_o<O);
A3x=A2x/(1.0*O);
A3y=A2y/(1.0*O);
A3z=A2z/(1.0*O);
calc_A3=true;
}while(true);
}
Opengl gyro_cube.c
/* Copyright (c) Mark J. Kilgard, 1997. */
/* This program is freely distributable without licensing fees
and is provided without guarantee or warrantee expressed or
implied. This program is -not- in the public domain. */
/* This program was requested by Patrick Earl; hopefully someone else
will write the equivalent Direct3D immediate mode program. */
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <math.h>
#include <GL/glut.h>
GLfloat light_diffuse[] = {1.0, 0.0, 0.0, 1.0}; /* Red diffuse light. */
GLfloat light_position[] = {1.0, 5.0, -1.0, 0.0}; /* Infinite light location. */
GLfloat n[6][3] = { /* Normals for the 6 faces of a cube. */
{-1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {1.0, 0.0, 0.0},
{0.0, -1.0, 0.0}, {0.0, 0.0, 1.0}, {0.0, 0.0, -1.0} };
GLint faces[6][4] = { /* Vertex indices for the 6 faces of a cube. */
{0, 1, 2, 3}, {3, 2, 6, 7}, {7, 6, 5, 4},
{4, 5, 1, 0}, {5, 6, 2, 1}, {7, 4, 0, 3} };
GLfloat v[8][3]; /* Will be filled in with X,Y,Z vertexes. */
//serial descriptor
int fd;
char *portname = "/dev/ttyUSB0";
int
set_interface_attribs (int fd, int speed, int parity)
{
struct termios tty;
memset (&tty, 0, sizeof tty);
if (tcgetattr (fd, &tty) != 0)
{
// error_message ("error %d from tcgetattr", errno);
return -1;
}
cfsetospeed (&tty, speed);
cfsetispeed (&tty, speed);
tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars
// disable IGNBRK for mismatched speed tests; otherwise receive break
// as \000 chars
tty.c_iflag &= ~IGNBRK; // disable break processing
tty.c_lflag = 0; // no signaling chars, no echo,
// no canonical processing
tty.c_oflag = 0; // no remapping, no delays
tty.c_cc[VMIN] = 0; // read doesn't block
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl
tty.c_cflag |= (CLOCAL | CREAD);// ignore modem controls,
// enable reading
tty.c_cflag &= ~(PARENB | PARODD); // shut off parity
tty.c_cflag |= parity;
tty.c_cflag &= ~CSTOPB;
tty.c_cflag &= ~CRTSCTS;
if (tcsetattr (fd, TCSANOW, &tty) != 0)
{
//error_message ("error %d from tcsetattr", errno);
return -1;
}
return 0;
}
void
set_blocking (int fd, int should_block)
{
struct termios tty;
memset (&tty, 0, sizeof tty);
if (tcgetattr (fd, &tty) != 0)
{
//error_message ("error %d from tggetattr", errno);
return;
}
tty.c_cc[VMIN] = should_block ? 1 : 0;
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
tcsetattr (fd, TCSANOW, &tty);
}
void
drawBox(void)
{
int i;
for (i = 0; i < 6; i++) {
glBegin(GL_QUADS);
glNormal3fv(&n[i][0]);
glVertex3fv(&v[faces[i][0]][0]);
glVertex3fv(&v[faces[i][1]][0]);
glVertex3fv(&v[faces[i][2]][0]);
glVertex3fv(&v[faces[i][3]][0]);
glEnd();
}
}
///////////////////////////////////////////////////////////////////////////
void
display(void)
{
char buffer1[100],buffer2[100],buffer3[100];
float A1x=0.0,A1y=0.0,A1z=0.0;
char line[200];
char *pch;
char *pch1;
char *pch2;
char in;
ssize_t length;
int i=0;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
printf("START\n");
do{
length = read(fd, &in, 1);
//printf("%d",length);
if(length==1){
line[i]=in;
i++;
}
//printf("%d",i);
}while( in!='\n');
line[i]='\0';
//printf("%s\n",line);
pch=strchr(line,'\t');
strncpy(buffer1,line,pch-line+1);
A1x=atof(buffer1);
pch1=strchr(pch+1,'\t');
strncpy(buffer2,pch,pch1-pch+1);
A1y=atof(buffer2);
pch2=strchr(pch1+1,'\n');
strncpy(buffer3,pch1,pch2-pch1+1);
A1z=atof(buffer3);
printf("%g\t%g\t%g\n",A1x,A1y,A1z);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(0.0, 0.0, 5.0, /* eye is at (0,0,5) */
0.0, 0.0, 0.0, /* center is at (0,0,0) */
0.0, 1.0, 0.); /* up is in positive Y direction */
//glTranslatef(0.0, 0.0, 1.0);
glRotatef(A1z, 0.0, 1.0, 0.0);
glRotatef(-A1x+90, 1.0, 0.0, 0.0);
drawBox();
glutSwapBuffers();
glutPostRedisplay();
}
////////////////////////////////////////////////////////////////////////////
void
init(void)
{
/* Setup cube vertex data. */
v[0][0] = v[1][0] = v[2][0] = v[3][0] = -1;
v[4][0] = v[5][0] = v[6][0] = v[7][0] = 1;
v[0][1] = v[1][1] = v[4][1] = v[5][1] = -1;
v[2][1] = v[3][1] = v[6][1] = v[7][1] = 1;
v[0][2] = v[3][2] = v[4][2] = v[7][2] = 1;
v[1][2] = v[2][2] = v[5][2] = v[6][2] = -1;
/* Enable a single OpenGL light. */
glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
glLightfv(GL_LIGHT0, GL_POSITION, light_position);
glEnable(GL_LIGHT0);
glEnable(GL_LIGHTING);
/* Use depth buffering for hidden surface elimination. */
glEnable(GL_DEPTH_TEST);
/* Setup the view of the cube. */
glMatrixMode(GL_PROJECTION);
gluPerspective( /* field of view in degree */ 40.0,
/* aspect ratio */ 1.0,
/* Z near */ 1.0, /* Z far */ 10.0);
glMatrixMode(GL_MODELVIEW);
gluLookAt(0.0, 0.0, 5.0, /* eye is at (0,0,5) */
0.0, 0.0, 0.0, /* center is at (0,0,0) */
0.0, 1.0, 0.); /* up is in positive Y direction */
/* Adjust cube position to be asthetic angle. */
//glTranslatef(0.0, 0.0, -1.0);
//glRotatef(60, 1.0, 0.0, 0.0);
//glRotatef(-20, 0.0, 0.0, 1.0);
}
int
main(int argc, char **argv)
{
fd = open (portname, O_RDWR | O_NOCTTY | O_SYNC);
if (fd < 0)
{
printf ("error opening port\n");
return 1;
}
printf("port opened...");
set_interface_attribs (fd, B115200, 0); // set speed to 115,200 bps, 8n1 (no parity)
set_blocking (fd, 0);
//printf("Writing port...\n");
//write (fd, "\n", 2); // send 7 character greeting
//usleep ((2) * 100);
printf("init glut...");
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
glutCreateWindow("red 3D lighted cube");
glutDisplayFunc(display);
init();
glutMainLoop();
return 0; /* ANSI C requires main to return int. */
}
No comments:
Post a Comment