OX
hahaha
Dependencies: mbed
Revision 2:ce3ee4bc8cf7, committed 2016-12-06
- Comitter:
- siwakon
- Date:
- Tue Dec 06 12:19:49 2016 +0000
- Parent:
- 1:d8ce226c8c2e
- Commit message:
- ddd
Changed in this revision
--- a/MPU9250.h Tue Dec 06 06:11:54 2016 +0000
+++ b/MPU9250.h Tue Dec 06 12:19:49 2016 +0000
@@ -225,8 +225,6 @@
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method
-Serial apc(USBTX,USBRX);
-
class MPU9250 {
protected:
@@ -250,7 +248,7 @@
char data_write[1];
data_write[0] = subAddress;
i2c.write(address, data_write, 1, 1); // no stop
- i2c.read(address, data, 1, 0);
+ i2c.read(address, data, 1, 0);
return data[0];
}
--- a/main.cpp Tue Dec 06 06:11:54 2016 +0000
+++ b/main.cpp Tue Dec 06 12:19:49 2016 +0000
@@ -1,7 +1,6 @@
#include "mbed.h"
#include "bmu.h"
-
InterruptIn lim(D3);
InterruptIn rotate(D4);
InterruptIn choose(D5);
@@ -124,38 +123,39 @@
{
for (int i = 0;i<siz;i++)
{
- mas.putc(char(arrayi[i]));
- wait(it);
+ mas.putc(char(arrayi[i]));UUpdate();
+ wait(it);UUpdate();
}
}
void getStr()
{
+ UUpdate();
while (mas.readable())
{
- elem = mas.getc();
- pc.printf("char is %d\n\r",elem);
+ elem = mas.getc();UUpdate();
+ pc.printf("char is %d\n\r",elem);UUpdate();
if ((elem == '\n'))
{
- pc.printf("string is %s\n\r",data);
+ pc.printf("string is %s\n\r",data);UUpdate();
for(int i=0;i<indx;i++)
{
if (indx > 2)
{
- dete[i] = data[i];
+ dete[i] = data[i];UUpdate();
}
}
for(int i=0;i<30;i++)
{
data[i] = '\0';
}
- indx = 0;
+ indx = 0;UUpdate();
break;
}else
{
- data[indx] = elem;
- indx += 1;
+ data[indx] = elem;UUpdate();
+ indx += 1;UUpdate();
}
}
@@ -165,71 +165,71 @@
if (t.read() > 1) {
- duino.putc(lis[23]);
+ duino.putc(lis[23]);UUpdate();
- timer.reset();
+ timer.reset();UUpdate();
}
}
void LWin(){ //ชนะ
if ( timer.read() > 1) {
//pc.printf("Writing!\n\r");
- duino.putc( lis[0]);
- duino.putc( lis[1]);
- duino.putc( lis[2]);
- timer.reset();
+ duino.putc( lis[0]);UUpdate();
+ duino.putc( lis[1]);UUpdate();
+ duino.putc( lis[2]);UUpdate();
+ timer.reset();UUpdate();
}
}
void LCountDown(int ixxii){ //นับเวลา
if ( timer.read() > 1) {
- duino.putc(ixxii);
- timer.reset();
+ duino.putc(ixxii);UUpdate();
+ timer.reset();UUpdate();
}
}
void LCorrect(){ //เช็คถูก
if ( timer.read() > 1) {
- //pc.printf("Writing!\n\r");
- duino.putc(lis[18]);
- timer.reset();
+ //pc.printf("Writing!\n\r");UUpdate();
+ duino.putc(lis[18]);UUpdate();
+ timer.reset();UUpdate();
}
}
void LError(){ //
if ( timer.read() > 1) {
- //pc.printf("Writing!\n\r");
- duino.putc( lis[19]);
- timer.reset();
+ //pc.printf("Writing!\n\r");UUpdate();
+ duino.putc( lis[19]);UUpdate();
+ timer.reset();UUpdate();
}
}
void LMapComplete(){ //ลงค่าเสร็จ
if ( timer.read() > 1) {
- //pc.printf("Writing!\n\r");
+ //pc.printf("Writing!\n\r");UUpdate();
- duino.putc( lis[20]);
+ duino.putc( lis[20]);UUpdate();
- timer.reset();}}
+ timer.reset();UUpdate();}}
void LWbox(){ //ตัวหลัก
if ( timer.read() > 1) {
- duino.putc( lis[21]);
- timer.reset();
+ duino.putc( lis[21]);UUpdate();
+ timer.reset();UUpdate();
}
}
void LRotateComplete(){ //หมุน
if ( timer.read() > 1) {
- //pc.printf("Writing!\n\r");
+ //pc.printf("Writing!\n\r");UUpdate();
- duino.putc( lis[22]);
+ duino.putc( lis[22]);UUpdate();
- timer.reset();
+ timer.reset();UUpdate();
}
}
@@ -239,11 +239,11 @@
if ( t.read() > 1) {
//pc.printf("Writing!\n\r");
- duino.putc( lis[3]);
- duino.putc( lis[4]);
- duino.putc( lis[5]);
- duino.putc( lis[6]);
- t.reset();
+ duino.putc( lis[3]);UUpdate();
+ duino.putc( lis[4]);UUpdate();
+ duino.putc( lis[5]);UUpdate();
+ duino.putc( lis[6]);UUpdate();
+ t.reset();UUpdate();
}
if ( duino.readable())
{
@@ -254,13 +254,13 @@
void Lose()
{
- LLose();
+ LLose();UUpdate();
Sent("los\n",0.1,4);
}
void Win()
{
- LWin();
+ LWin();UUpdate();
Sent("lwin\n",0.1,4);
}
@@ -334,20 +334,20 @@
{
if (xx != -1)
{
- WMbox[0] = xx;
- WMbox[1] = yy;
- WMbox[2] = zz;
- WMbox[3] = lx;
- WMbox[4] = ly;
- WMbox[5] = lz;
+ WMbox[0] = xx;UUpdate();
+ WMbox[1] = yy;UUpdate();
+ WMbox[2] = zz;UUpdate();
+ WMbox[3] = lx;UUpdate();
+ WMbox[4] = ly;UUpdate();
+ WMbox[5] = lz;UUpdate();
}else
{
- WMbox[0] = lx;
- WMbox[1] = ly;
- WMbox[2] = lz;
- WMbox[3] = xx;
- WMbox[4] = yy;
- WMbox[5] = zz;
+ WMbox[0] = lx;UUpdate();
+ WMbox[1] = ly;UUpdate();
+ WMbox[2] = lz;UUpdate();
+ WMbox[3] = xx;UUpdate();
+ WMbox[4] = yy;UUpdate();
+ WMbox[5] = zz;UUpdate();
}
}
@@ -355,14 +355,14 @@
{
if (box)
{
- WMbox[0] = -1;
- WMbox[1] = -1;
- WMbox[2] = -1;
+ WMbox[0] = -1;UUpdate();
+ WMbox[1] = -1;UUpdate();
+ WMbox[2] = -1;UUpdate();
}else
{
- WMbox[3] = -1;
- WMbox[4] = -1;
- WMbox[5] = -1;
+ WMbox[3] = -1;UUpdate();
+ WMbox[4] = -1;UUpdate();
+ WMbox[5] = -1;UUpdate();
}
}
@@ -376,10 +376,10 @@
{
for(int k=0;k<5;k++)
{
- A = ((i-2)* Cross(pitch))+((k-2)* Zine(pitch));
- B = ((j-2)* Cross(yaw))+((i-2)* Zine(pitch)* Zine(yaw))-((k-2)* Zine(yaw)* Cross(pitch));
- C = ((j-2)* Zine(yaw))-((i-2)* Cross(yaw)* Zine(pitch))+((k-2)* Cross(yaw)* Cross(pitch));
- rmap[i+2][j+2][k+2]= map[i][j][k];
+ A = ((i-2)* Cross(pitch))+((k-2)* Zine(pitch));UUpdate();
+ B = ((j-2)* Cross(yaw))+((i-2)* Zine(pitch)* Zine(yaw))-((k-2)* Zine(yaw)* Cross(pitch));UUpdate();
+ C = ((j-2)* Zine(yaw))-((i-2)* Cross(yaw)* Zine(pitch))+((k-2)* Cross(yaw)* Cross(pitch));UUpdate();
+ rmap[i+2][j+2][k+2]= map[i][j][k];UUpdate();
}
}
}
@@ -391,10 +391,10 @@
{
for(int k=0;k<5;k++)
{
- A = ((i-2)* Cross(yaw)) + ((j-2)* Zine(role)* Zine(yaw)) +((k-2)* Zine(yaw)* Cross(role));
- B = ((j-2)* Cross(role))-((k-2)* Zine(role));
- C = -((i-2)* Zine(yaw))+((j-2)* Cross(yaw)* Zine(role))+((k-2)* Cross(yaw)* Cross(role));
- rmap[i+2][j+2][k+2]= map[i][j][k];
+ A = ((i-2)* Cross(yaw)) + ((j-2)* Zine(role)* Zine(yaw)) +((k-2)* Zine(yaw)* Cross(role));UUpdate();
+ B = ((j-2)* Cross(role))-((k-2)* Zine(role));UUpdate();
+ C = -((i-2)* Zine(yaw))+((j-2)* Cross(yaw)* Zine(role))+((k-2)* Cross(yaw)* Cross(role));UUpdate();
+ rmap[i+2][j+2][k+2]= map[i][j][k];UUpdate();
}
}
}
@@ -406,10 +406,10 @@
{
for(int k=0;k<5;k++)
{
- A = ((i-2)* Cross(yaw))-((j-2)* Zine(yaw));
- B = ((i-2)* Zine(yaw))+((j-2)* Cross(yaw));
- C = (k-2);
- rmap[i+2][j+2][k+2]= map[i][j][k];
+ A = ((i-2)* Cross(yaw))-((j-2)* Zine(yaw));UUpdate();
+ B = ((i-2)* Zine(yaw))+((j-2)* Cross(yaw));UUpdate();
+ C = (k-2);UUpdate();
+ rmap[i+2][j+2][k+2]= map[i][j][k];UUpdate();
}
}
}
@@ -420,7 +420,7 @@
{
for(int k=0;k<5;k++)
{
- map[i][j][k]= rmap[i][j][k];
+ map[i][j][k]= rmap[i][j][k];UUpdate();
}
}
}
@@ -434,34 +434,34 @@
case '0':
i-- , j-- , k--;
if( map[i][j][k] == 0){
- map[i][j][k] = 5; // 5 = 'x'
- turn++;
+ map[i][j][k] = 5;UUpdate(); // 5 = 'x'
+ turn++;UUpdate();
}
else{
- Lose(); // ลงไมไ่ด้
+ Lose();UUpdate(); // ลงไมไ่ด้
}
case '1':
i-- , j-- , k--;
if( map[i][j][k] == 0){
- map[i][j][k] = 6; // 6 = 'o'
- turn++;
+ map[i][j][k] = 6;UUpdate(); // 6 = 'o'
+ turn++;UUpdate();
}
else{
- Lose(); // ลงไมไ่ด้
+ Lose();UUpdate(); // ลงไมไ่ด้
}
}
if(i == 5 or j == 5 or k == 5){ // ลงเกิน map
- Lose();
+ Lose();UUpdate();
}
for(i=0 ; i<5 ; i++){ //แนว k
for(j=0 ; j<5 ; j++){
for(k=0 ; k<3 ; k++){
if( map[i][j][k] and map[i][j][k+1] and map[i][j][k+2] == 5){
- Win();
+ Win();UUpdate();
}
else if( map[i][j][k] and map[i][j][k+1] and map[i][j][k+2] == 6){
- Win();
+ Win();UUpdate();
}
}
}
@@ -471,10 +471,10 @@
for(k=0 ; k<5 ; k++){
for(j=0 ; j<3 ; j++){
if( map[i][j][k] and map[i][j+1][k] and map[i][j+2][k] == 5){
- Win();
+ Win();UUpdate();
}
else if( map[i][j][k] and map[i][j+1][k] and map[i][j+2][k] == 6){
- Win();
+ Win();UUpdate();
}
}
}
@@ -484,10 +484,10 @@
for(k=0 ; k<5 ; k++){
for(i=0 ; i<3 ; i++){
if( map[i][j][k] and map[i+1][j][k] and map[i+2][j][k] == 5){
- Win();
+ Win();UUpdate();
}
else if( map[i][j][k] and map[i+1][j][k] and map[i+2][j][k] == 6){
- Win();
+ Win();UUpdate();
}
}
}
@@ -497,10 +497,10 @@
for(j=0 ; j<3 ; j++){
for(k=0 ; k<3 ; k++){
if( map[i][j][k] and map[i][j+1][k+1] and map[i][j+2][k+2] == 5){
- Win();
+ Win();UUpdate();
}
else if( map[i][j][k] and map[i][j+1][k+1] and map[i][j+2][k+2] == 6){
- Win();
+ Win();UUpdate();
}
}
}
@@ -510,10 +510,10 @@
for(i=0 ; i<3 ; i++){
for(k=0 ; k<3 ; k++){
if( map[i][j][k] and map[i+1][j][k+1] and map[i+2][j][k+2] == 5){
- Win();
+ Win();UUpdate();
}
else if( map[i][j][k] and map[i+1][j][k+1] and map[i+2][j][k+2] == 6){
- Win();
+ Win();UUpdate();
}
}
}
@@ -523,10 +523,10 @@
for(i=0 ; i<3 ; i++){
for(j=0 ; j<3 ; j++){
if( map[i][j][k] and map[i+1][j+1][k] and map[i+2][j+2][k] == 5){
- Win();
+ Win();UUpdate();
}
else if( map[i][j][k] and map[i+1][j+1][k] and map[i+2][j+2][k] == 6){
- Win();
+ Win();UUpdate();
}
}
}
@@ -536,10 +536,10 @@
for(j=0 ; j<3 ; j++){
for(k=4 ; k>1 ; k--){
if( map[i][j][k] and map[i][j+1][k-1] and map[i][j+2][k-2] == 5){
- Win();
+ Win();UUpdate();
}
else if( map[i][j][k] and map[i][j+1][k-1] and map[i][j+2][k-2] == 6){
- Win();
+ Win();UUpdate();
}
}
}
@@ -549,10 +549,10 @@
for(i=0 ; i<3 ; i++){
for(k=4 ; k>1 ; k--){
if( map[i][j][k] and map[i+1][j][k-1] and map[i+2][j][k-2] == 5){
- Win();
+ Win();UUpdate();
}
else if( map[i][j][k] and map[i+1][j][k-1] and map[i+2][j][k-2] == 6){
- Win();
+ Win();UUpdate();
}
}
}
@@ -562,10 +562,10 @@
for(i=0 ; i<3 ; i++){
for(j=4 ; j>1 ; j--){
if( map[i][j][k] and map[i+1][j-1][k] and map[i+2][j-2][k] == 5){
- Win();
+ Win();UUpdate();
}
else if( map[i][j][k] and map[i+1][j-1][k] and map[i+2][j-2][k] == 6){
- Win();
+ Win();UUpdate();
}
}
}
@@ -574,10 +574,10 @@
for(j=0 ; j<3 ; j++){
for(k=0 ; k<3 ; k++){
if( map[i][j][k] and map[i+1][j+1][k+1] and map[i+2][j+2][k+2] == 5){
- Win();
+ Win();UUpdate();
}
else if( map[i][j][k] and map[i+1][j+1][k+1] and map[i+2][j+2][k+2] == 6){
- Win();
+ Win();UUpdate();
}
}
}
@@ -586,10 +586,10 @@
for(j=0 ; j<3 ; j++){
for(k=0 ; k<3 ; k++){
if( map[i][j][k] and map[i-1][j+1][k+1] and map[i-2][j+2][k+2] == 5){
- Win();
+ Win();UUpdate();
}
else if( map[i][j][k] and map[i-1][j+1][k+1] and map[i-2][j+2][k+2] == 6){
- Win();
+ Win();UUpdate();
}
}
}
@@ -598,10 +598,10 @@
for(j=4 ; j>1 ; j--){
for(k=0 ; k<3 ; k++){
if( map[i][j][k] and map[i-1][j-1][k+1] and map[i-2][j-2][k+2] == 5){
- Win();
+ Win();UUpdate();
}
else if( map[i][j][k] and map[i-1][j-1][k+1] and map[i-2][j-2][k+2] == 6){
- Win();
+ Win();UUpdate();
}
}
}
@@ -610,10 +610,10 @@
for(j=4 ; j>1 ; j--){
for(k=0 ; k<3 ; k++){
if( map[i][j][k] and map[i+1][j-1][k+1] and map[i+2][j-2][k+2] == 5){
- Win();
+ Win();UUpdate();
}
else if( map[i][j][k] and map[i+1][j-1][k+1] and map[i+2][j-2][k+2] == 6){
- Win();
+ Win();UUpdate();
}
}
}
@@ -631,14 +631,14 @@
for( B=0 ; B<5 ; B++){
for( C=0 ; C<5 ; C++){
if( map[ A][ B][ C] == 5){
- Check1[ A][ B][ C] = 1;
+ Check1[ A][ B][ C] = 1;UUpdate();
}
else if( map[ A][ B][ C] == 6){
- Check2[ A][ B][ C] = 1;
+ Check2[ A][ B][ C] = 1;UUpdate();
}
else{
- Check1[ A][ B][ C] = 0;
- Check2[ A][ B][ C] = 0;
+ Check1[ A][ B][ C] = 0;UUpdate();
+ Check2[ A][ B][ C] = 0;UUpdate();
}
}
}
@@ -661,7 +661,7 @@
UUpdate();
}*/
//LInitial(); // set initial pattern in led matrix.
- //wait(5); // wait 5 second for putting the box down to set initial position of imu.
+ wait(5); // wait 5 second for putting the box down to set initial position of imu.
//imu.Initial(); // set initial for imu.
//Update.attach(&UUpdate,0.1); //timer interrupt for recieving bluetooth.
//while((dete[0]!='i')||(dete[1]!='n')||(dete[2]!='i')||(dete[3]!='t'))
@@ -671,13 +671,13 @@
t.start();
while (1) // void loop.
{
- state = 1;
+ state = 1;UUpdate();
UUpdate();
if (buttonState[0] == 1) // if the boxes are connected.
{
- x = GetCoordinateX();
- y = GetCoordinateY();
- z = GetCoordinateZ(); // get the position of main box which linking with map or the box not be choosen.
+ x = GetCoordinateX();UUpdate();
+ y = GetCoordinateY();UUpdate();
+ z = GetCoordinateZ();UUpdate(); // get the position of main box which linking with map or the box not be choosen.
/*Sent("helr\n",0.1,5); // tell the other box, that we want helper roll.
while((dete[0]!='h')||(dete[1]!='e')||(dete[2]!='l')||(dete[3]!='r'))
{
@@ -698,62 +698,62 @@
helperYaw = int( ((int(dete[4+ii])-48)*100)+((int(dete[5+ii])-48)*10)+(int(dete[6+ii])-48) ); //get roll from another box.*/
//Sent("ok\n",0.1,3); //free the otherbox.
- errorie = imu.Getposition(x,y,z);//,helperRoll,helperPitch,helperYaw); // calculate position from imu.
+ errorie = imu.Getposition(x,y,z);//,helperRoll,helperPitch,helperYaw);UUpdate(); // calculate position from imu.
if (errorie)
{
- LError();
+ LError();UUpdate();
while(buttonState[0] == 1)
{
UUpdate();
}
}else
{
- lx = x;
- ly = y;
- lz = z;
- x = imu.X_pos;
- y = imu.Y_pos;
- z = imu.Z_pos; // get new possition.
+ lx = x;UUpdate();
+ ly = y;UUpdate();
+ lz = z;UUpdate();
+ x = imu.X_pos;UUpdate();
+ y = imu.Y_pos;UUpdate();
+ z = imu.Z_pos;UUpdate(); // get new possition.
- UpdateMap(x,y,z,turn);//,helperRoll); //update new position in map.
+ UpdateMap(x,y,z,turn);UUpdate();//,helperRoll); //update new position in map.
- ChangeOX(x,y,z,lx,ly,lz);
+ ChangeOX(x,y,z,lx,ly,lz);UUpdate();
Sent("mapc\n",0.1,4); //map complete.
- LMapComplete(); // show that this position is ok.
+ LMapComplete();UUpdate(); // show that this position is ok.
while(1)
{
UUpdate();
- anach = chooseAnother() ;// wheather thr others box is choosen.
+ anach = chooseAnother() ;UUpdate();// wheather thr others box is choosen.
if ((buttonState[2]==1) && (!anach))
{
Sent("ch1\n",0.1,4);
- Choose(1); // choose main box.
+ Choose(1);UUpdate(); // choose main box.
}else if((buttonState[2]==0) && (anach))
{
- Choose(0); // choose another box.
+ Choose(0);UUpdate(); // choose another box.
}else if(buttonState[2]==0)
{
if((buttonState[1]==0)||(anotherRotate())) // click buutton some rotate box.
{
- state = 3;
+ state = 3;UUpdate();
while((buttonState[1]==0)&&(anotherRotate()))
{
UUpdate();
}
- yyaaww = imu.Rotate(); // check wheater rotating is error.
- MapRotate(0,0,yyaaww);//imu.Rotate_Yaw); // private variable/arttribute in imu class.
- LRotateComplete();
+ yyaaww = imu.Rotate();UUpdate(); // check wheater rotating is error.
+ MapRotate(0,0,yyaaww);UUpdate();//imu.Rotate_Yaw); // private variable/arttribute in imu class.
+ LRotateComplete();UUpdate();
}
} //close choose or rotate.
} //close while true.
- turn ++;
+ turn ++;UUpdate();
Sent("T++\n",0.1,4);
- t.start();
- state = 2;
+ t.start();UUpdate();
+ state = 2;UUpdate();
//LChooseComplete();
} // close not error.
@@ -761,18 +761,18 @@
{
if (county == 0) // time out.
{
- t.stop();
- Lose(); // lose state
+ t.stop();UUpdate();
+ Lose();UUpdate(); // lose state
}else if(turn != 0)
{
- t.stop();
- if (t.read() > 1.0);
+ t.stop();UUpdate();
+ if (t.read() > 1.0)
{
- t.start();
- county -= 1; // time decreasing.
+ t.start();UUpdate();
+ county -= 1;UUpdate(); // time decreasing.
}
} // close check time
- LCountDown(county);
+ LCountDown(county);UUpdate();
Sent("Tur\n",0.1,4);
} // close boxes aren't connect.
--- a/zmu9250.h Tue Dec 06 06:11:54 2016 +0000
+++ b/zmu9250.h Tue Dec 06 12:19:49 2016 +0000
@@ -1,31 +1,28 @@
-#ifndef ZMU9250_H_
-#define ZMU9050_H_
-
-#endif
-
#include "mbed.h"
#include "MPU9250.h"
#include "math.h"
#include "kalman.h"
Serial aa(USBTX,USBRX);
-class ZMU9250{
-
+
+
+class ZMU9250
+{
public:
- ZMU9250::ZMU9250()
+ ZMU9250()
{
//Set up I2C
- //aa.printf("null\n");
i2c.frequency(400000); // use fast (400 kHz) I2C
this->t.start();
// Read the WHO_AM_I register, this is a good test of communication
+ uint8_t whoami = this->mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
- uint8_t whoami = this->mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
- aa.printf("whoami = %d\n\r ",whoami);
- if ((whoami == 0x71))//||(whoami == 0x71)||(true)) // WHO_AM_I should always be 0x68
+
+ if ((whoami == 0x71)||(whoami == 0x73)) // WHO_AM_I should always be 0x68
{
+
wait(1);
this->mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
this->mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
@@ -34,18 +31,14 @@
this->mpu9250.initMPU9250();
this->mpu9250.initAK8963(magCalibration);
wait(1);
-
}
else
{
- //aa.printf("forever\n");
while(1) ; // Loop forever if communication doesn't happen
}
- //aa.printf("first\n");
this->mpu9250.getAres(); // Get accelerometer sensitivity
this->mpu9250.getGres(); // Get gyro sensitivity
this->mpu9250.getMres(); // Get magnetometer sensitivity
- //aa.printf("second\n");
//magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
//magbias[1] = +120.; // User environmental x-axis correction in milliGauss
//magbias[2] = +125.; // User environmental x-axis correction in milliGauss
@@ -54,9 +47,8 @@
magbias[2] = +125; // User environmental x-axis correction in milliGauss
}
- void ZMU9250::Update()
+ void Update()
{
- // aa.printf("hellow\n\r");
if(this->mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
this->mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
// Now we'll calculate the accleration value into actual g's
@@ -71,12 +63,10 @@
this->mpu9250.readMagData(magCount); // Read the x/y/z adc values
// Calculate the magnetometer values in milliGauss
// Include factory calibration per data sheet and user environmental corrections
- //aa.printf("ten\n\r");
mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]+360.0f; // get actual magnetometer value, this depends on scale being set
my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]-210.0f;
mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
- //aa.printf("eleven\n\r");
- //aa.printf("x %f\ty %f\tz %f\n\r",gyroCount[0],gy,gz);
+ //aa.printf("x %f\ty %f\tz %f\n",mx,my,mz);
} // end if one
@@ -85,9 +75,9 @@
lastUpdate = Now;
this->sum += deltat;
sumCount++;
+ this->mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+
// Pass gyro rate as rad/s
- this->mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
- //this->mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
/*if((rand()%20)>=0)
{
this->mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
@@ -115,7 +105,10 @@
yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
//yaw = atan2(2.0f * (q[0] * q[2] + q[0] * q[3]), 1 - 2 * ( q[2] * q[2] + q[3] * q[3]));
pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+
+ //pitch = atan2(2.0f * (q[1] * q[3] - q[0] * q[2]),q[0]*q[0]-q[1]*q[1]+q[2]*q[2]-q[3]*q[3]);
roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+ //pitch = atan2(sin(roll)*(q[1]*q[3]-q[0]*q[2]),q[1]*q[2]+q[0]*q[3]);
pitch *= 180.0f / PI;
yaw *= 180.0f / PI;
//yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
@@ -137,17 +130,17 @@
}
- float ZMU9250::Roll()
+ float Roll()
{
return roll_x;
}
- float ZMU9250::Pitch()
+ float Pitch()
{
return pitch_y;
}
- float ZMU9250::Yaw()
+ float Yaw()
{
return yaw_z;
}
@@ -168,4 +161,3 @@
};
-