Revision 1:d8ce226c8c2e, committed 2016-12-06

Comitter:

arthicha

Date:

Tue Dec 06 06:11:54 2016 +0000

Parent:

0:a291977ec0b1

Child:

2:ce3ee4bc8cf7

Commit message:

;UUpdate;

Changed in this revision

--- a/bmu.h	Mon Dec 05 18:31:43 2016 +0000
+++ b/bmu.h	Tue Dec 06 06:11:54 2016 +0000
@@ -2,12 +2,15 @@
 #include "zmu9250.h"
 #include "math.h"
 
+Serial aaa (USBTX,USBRX);
 
-ZMU9250 a;
+
 class bmuimu
 
     {
         
+    private:
+            ZMU9250 a;
               
         
     public:
@@ -73,12 +76,13 @@
                 return true ;
          }   
     
-      void bmuimu::update() {
+      void bmuimu::bmuupdate() {
+                this->a.Update();
+                this->yaw_z = this->a.Yaw();
+                this->pitch_y = this->a.Pitch();
+                this->roll_x = this->a.Roll();
+                aaa.printf("%d , %d and %d\n\r",this->roll_x,this->pitch_y,this->yaw_z);
                 
-                a.Update();
-                this->yaw_z = a.Yaw();
-                this->pitch_y = a.Pitch();
-                this->roll_x = a.Roll();
        
        }
        

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.cpp	Tue Dec 06 06:11:54 2016 +0000
@@ -0,0 +1,52 @@
+#include "kalman.h"
+// derived from http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
+kalman::kalman(void){
+    P[0][0]     = 0;
+    P[0][1]     = 0;
+    P[1][0]     = 0;
+    P[1][1]     = 0;
+    
+    angle       = 0;
+    bias        = 0;
+    
+    Q_angle     = 0.001;
+    Q_gyroBias  = 0.003;
+    R_angle     = 0.03;     
+}
+
+double kalman::getAngle(double newAngle, double newRate, double dt){
+    //predict the angle according to the gyroscope
+    rate         = newRate - bias;
+    angle        = dt * rate;
+    //update the error covariance
+    P[0][0]     += dt * (dt * P[1][1] * P[0][1] - P[1][0] + Q_angle);
+    P[0][1]     -= dt * P[1][1];
+    P[1][0]     -= dt * P[1][1];
+    P[1][1]     += dt * Q_gyroBias;
+    //difference between the predicted angle and the accelerometer angle
+    y            = newAngle - angle;
+    //estimation error
+    S            = P[0][0] + R_angle;
+    //determine the kalman gain according to the error covariance and the distrust
+    K[0]         = P[0][0]/S;
+    K[1]         = P[1][0]/S;
+    //adjust the angle according to the kalman gain and the difference with the measurement
+    angle       += K[0] * y;
+    bias        += K[1] * y;
+    //update the error covariance
+    P[0][0]     -= K[0] * P[0][0];
+    P[0][1]     -= K[0] * P[0][1];
+    P[1][0]     -= K[1] * P[0][0];
+    P[1][1]     -= K[1] * P[0][1];
+
+    return angle;
+}
+void kalman::setAngle(double newAngle) { angle = newAngle; };
+void kalman::setQangle(double newQ_angle) { Q_angle = newQ_angle; };
+void kalman::setQgyroBias(double newQ_gyroBias) { Q_gyroBias = newQ_gyroBias; };
+void kalman::setRangle(double newR_angle) { R_angle = newR_angle; };
+
+double kalman::getRate(void) { return rate; };
+double kalman::getQangle(void) { return Q_angle; };
+double kalman::getQbias(void) { return Q_gyroBias; };
+double kalman::getRangle(void) { return R_angle; };
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.h	Tue Dec 06 06:11:54 2016 +0000
@@ -0,0 +1,36 @@
+#ifndef _kalman_H
+#define _kalman_H
+// derived from http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
+class kalman
+{
+public:
+    kalman(void);
+    double getAngle(double newAngle, double newRate, double dt);
+    
+    void setAngle(double newAngle);
+    void setQangle(double newQ_angle);
+    void setQgyroBias(double newQ_gyroBias);
+    void setRangle(double newR_angle);
+    
+    double getRate(void);
+    double getQangle(void);
+    double getQbias(void);
+    double getRangle(void);
+    
+
+private:
+    double P[2][2];         //error covariance matrix
+    double K[2];            //kalman gain
+    double y;               //angle difference
+    double S;               //estimation error
+
+    double rate;            //rate in deg/s
+    double angle;           //kalman angle
+    double bias;            //kalman gyro bias
+
+    double Q_angle;         //process noise variance for the angle of the accelerometer
+    double Q_gyroBias;      //process noise variance for the gyroscope bias
+    double R_angle;         //measurement noise variance 
+};
+
+#endif
\ No newline at end of file

--- a/main.cpp	Mon Dec 05 18:31:43 2016 +0000
+++ b/main.cpp	Tue Dec 06 06:11:54 2016 +0000
@@ -5,7 +5,7 @@
 InterruptIn lim(D3);
 InterruptIn rotate(D4);
 InterruptIn choose(D5);
-Ticker flipper;
+//Ticker flipper;
 Ticker Update;
 
 bmuimu imu;
@@ -117,7 +117,7 @@
 
 void UUpdate()
 {
-    imu.update();   
+    imu.bmuupdate();   
 }
 
 void Sent(char arrayi[],float it,int siz)
@@ -646,42 +646,33 @@
             }
             
             
-void LInitial(){  //ลงค่าเสร็จ
-    if (timer.read() > 1) {
-        
-        
-        duino.putc(lis[20]);
-        
-        
-        timer.reset();
-                }
-    if (duino.readable()){
-            }         
-}
 
 
 int main() // main program of 
 {
-    lim.rise(&Rise1);
+    /*lim.rise(&Rise1);
     lim.fall(&Fall1);
     rotate.rise(&Rise2);
     rotate.fall(&Fall2);
     choose.rise(&Rise3);
-    choose.fall(&Fall3);
-    
-    LInitial(); // set initial pattern in led matrix.
-    wait(5); // wait 5 second for putting the box down to set initial position of imu.
+    choose.fall(&Fall3);*/
+    /*for(int i=0;i<200;i++)
+    {
+        UUpdate();
+    }*/
+    //LInitial(); // set initial pattern in led matrix.
+    //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,1.0); //timer interrupt for recieving bluetooth. 
-    while((dete[0]!='i')||(dete[1]!='n')||(dete[2]!='i')||(dete[3]!='t'))
-    {
-    }
+    //Update.attach(&UUpdate,0.1); //timer interrupt for recieving bluetooth. 
+    //while((dete[0]!='i')||(dete[1]!='n')||(dete[2]!='i')||(dete[3]!='t'))
+    //{
+    //}
     LWbox(); // set led to tell the player that the game is ready and this is the woman-box.
     t.start();
     while (1) // void loop.
     {
         state = 1;
-        
+        UUpdate();
         if (buttonState[0] == 1) // if the boxes are connected.
         {
             x = GetCoordinateX();
@@ -713,7 +704,7 @@
                 LError();
                 while(buttonState[0] == 1)
                 {
-                       
+                   UUpdate();    
                 }
             }else
             {
@@ -732,6 +723,7 @@
                     LMapComplete(); // show that this position is ok.
                     while(1)
                     {
+                        UUpdate();
                         anach = chooseAnother() ;// wheather thr others box is choosen. 
                         if ((buttonState[2]==1) && (!anach)) 
                         {
@@ -749,7 +741,7 @@
                                 state = 3;
                                 while((buttonState[1]==0)&&(anotherRotate()))
                                 {
-                                    
+                                    UUpdate();
                                 }
                                 
                                 yyaaww = imu.Rotate(); // check wheater rotating is error. 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main2.h	Tue Dec 06 06:11:54 2016 +0000
@@ -0,0 +1,337 @@
+#include "mbed.h"
+#include "bmu.h"
+
+
+InterruptIn lim(D3);
+InterruptIn rotate(D4);
+InterruptIn choose(D5);
+Ticker flipper;
+Ticker Update;
+
+bmuimu imu;
+
+Serial pc(USBTX,USBRX);
+Serial mas(D8,D2);
+Serial duino(PA_11, PA_12);
+
+Timer t;
+Timer timer;
+
+//*********************************************************************************************************************************
+//*********************************************************************************************************************************
+//*********************************************************************************************************************************
+//                                                          variable
+//*********************************************************************************************************************************
+//*********************************************************************************************************************************
+//*********************************************************************************************************************************
+bool anach = false;
+bool buttonState[3] = {0,0,0}; // button limit switchm rotate and choose state respectively.
+unsigned int county = 10; // county down.
+char data[50];
+char elem;
+int yyaaww;
+bool errorie = false; // store error in ambigous imu data. 
+int helperRoll = 0, helperPitch=0,helperYaw=0;
+unsigned int indx = 0;
+unsigned int state = 0;
+int dete[50];
+int lis[50] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30};
+
+// state    0 = initial / start.
+//          1 = waiting for limit switch.
+
+unsigned int turn = 0; // turn.
+
+unsigned int x = 0, y = 0, z = 0,lx=0,ly=0,lz=0;
+int winStatus = 0;
+
+
+
+int A , B , C , player , button; 
+int countDown = 9;
+int WMbox[6];
+int map[5][5][5],rmap[5][5][5]; //store map.
+int Check1[5][5][5] , Check2[5][5][5]; //convert to x or o map.
+
+void Rise(int but)
+{
+    buttonState[but-3] = 1;
+}
+
+void Fall(int but)
+{
+    buttonState[but-3] = 0;   
+}
+
+void Sent(char arrayi[],float it,int siz)
+{
+    for (int i = 0;i<siz;i++)
+        {
+            mas.putc(arrayi[i]);
+            wait(it);
+        }   
+}
+
+void LInitial(){  //ลงค่าเสร็จ
+    if (timer.read() > 1) {
+        
+        
+        duino.putc(lis[20]);
+        
+        
+        timer.reset();
+                }
+    if (duino.readable()){
+            }         
+}
+
+void getStr()
+{
+    while (mas.readable())
+    {
+        
+        elem = mas.getc();
+        pc.printf("char is %d\n\r",elem);
+        if ((elem == '\n'))
+        {
+            pc.printf("string is %s\n\r",data);   
+            for(int i=0;i<indx;i++)
+            {
+                if (indx > 2)
+                {
+                    dete[i] = data[i];      
+                }
+            }
+            for(int i=0;i<30;i++)
+            {
+                data[i] = '\0';
+            }
+            indx = 0;
+            break;
+        }else
+        {
+            data[indx] = elem;
+            indx += 1;
+        }
+            
+    }  
+}
+bool chooseAnother()
+{
+    Sent("cho\n",0.1,4);
+    while(1)
+    {
+        if ((dete[0]=='c')&&(dete[1]=='h')&&(dete[2]=='0'))   
+        {
+            return true;
+        }else if ((dete[0]=='c')&&(dete[1]=='h')&&(dete[2]=='x')) 
+        {
+            return false;
+        }
+    }
+}
+
+void Rise1()
+{
+    buttonState[0] = 1;
+}
+
+void Fall1()
+{
+    buttonState[0] = 0;   
+}
+void Rise2()
+{
+    buttonState[1] = 1;
+}
+
+void Fall2()
+{
+    buttonState[1] = 0;   
+}
+void LLose(){ //แพ้
+            
+            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();
+                }
+            if ( duino.readable())
+                {
+                //pc.printf("recieving %c",duino.getc());
+                }
+         
+            }
+void Lose()
+{
+    LLose();
+    Sent("los\n",0.1,4);
+}
+
+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();
+ }
+ }
+void Win()
+{
+    LWin();
+    Sent("lwin\n",0.1,4);
+}
+void Rise3()
+{
+    buttonState[2] = 1;
+}
+
+void Fall3()
+{
+    buttonState[2] = 0;   
+}
+bool anotherRotate()
+{
+    Sent("rot\n",0.1,4);
+    while(1)
+    {
+        if ((dete[0]=='r')&&(dete[1]=='o')&&(dete[2]=='t'))   
+        {
+            return true;
+        }else if ((dete[0]=='r')&&(dete[1]=='o')&&(dete[2]=='x')) 
+        {
+            return false;
+        }
+    }
+}
+void LCountDown(int ixxii){  //นับเวลา
+    if ( timer.read() > 1) {
+        duino.putc(ixxii);
+        timer.reset();
+                }
+    }
+void LMapComplete(){  //ลงค่าเสร็จ
+        if ( timer.read() > 1) {
+                //pc.printf("Writing!\n\r");
+                
+                
+                 duino.putc( lis[20]);
+                
+                
+                 timer.reset();}}
+
+int    GetCoordinateX()
+        {
+            if ( WMbox[0] != -1)
+            {
+              return  WMbox[0];   
+            }else
+            {
+                return  WMbox[3];   
+            }
+        }
+        
+int   GetCoordinateY()
+        {
+            if ( WMbox[1] != -1)
+            {
+              return  WMbox[1];   
+            }else
+            {
+                return  WMbox[4];   
+            }
+        }
+        
+int    GetCoordinateZ()
+        {
+            if ( WMbox[2] != -1)
+            {
+              return  WMbox[2];   
+            }else
+            {
+                return  WMbox[5];   
+            }
+        }
+        
+void   Choose(int box)
+        {
+            if (box)
+            {
+                 WMbox[0] = -1;
+                 WMbox[1] = -1;
+                 WMbox[2] = -1;   
+            }else
+            {
+                 WMbox[3] = -1;
+                 WMbox[4] = -1;
+                 WMbox[5] = -1;   
+            }
+        }
+        
+        
+
+
+
+
+
+int main()
+{
+    lim.rise(&Rise1);
+    lim.fall(&Fall1);
+    rotate.rise(&Rise2);
+    rotate.fall(&Fall2);
+    choose.rise(&Rise3);
+    choose.fall(&Fall3);
+    flipper.attach(&getStr,1.0); //timer interrupt for recieving bluetooth. 
+    LInitial(); // set initial pattern in led matrix.
+    wait(5); // wait 5 second for putting the box down to set initial position of imu.
+    
+    while(1){
+        
+        if ((dete[0]=='l')&&(dete[1]=='o')&&(dete[2]=='s')){
+            Lose();
+            
+            break;
+            }
+        else if ((dete[0]=='l')&&(dete[1]=='w')&&(dete[2]=='i')&&(dete[3]=='n')){
+            Win();
+            
+            break;
+            }  
+        else if ((dete[0]=='c')&&(dete[1]=='h')&&(dete[2]=='o')){
+            chooseAnother();
+            } 
+        else if ((dete[0]=='r')&&(dete[1]=='o')&&(dete[2]=='t')){
+            anotherRotate();
+            } 
+        else if ((dete[0]=='h')&&(dete[1]=='e')&&(dete[2]=='l')&&(dete[3]=='r')){
+            GetCoordinateX();
+            } 
+        else if ((dete[0]=='h')&&(dete[1]=='e')&&(dete[2]=='l')&&(dete[3]=='p')){
+            GetCoordinateY();
+            } 
+        else if ((dete[0]=='h')&&(dete[1]=='e')&&(dete[2]=='l')&&(dete[3]=='y')){
+            GetCoordinateZ();
+            } 
+        else if ((dete[0]=='o')&&(dete[1]=='k')){
+            LMapComplete();
+            } 
+        else if ((dete[0]=='m')&&(dete[1]=='a')&&(dete[2]=='p')&&(dete[3]=='c')){
+            LMapComplete();
+            } 
+        else if ((dete[0]=='T')&&(dete[1]=='+')&&(dete[2]=='+')){
+            turn += 1;
+            } 
+        else if ((dete[0]=='c')&&(dete[1]=='h')&&(dete[2]=='1')){
+            Choose(1);
+            } 
+        else if ((dete[0]=='T')&&(dete[1]=='u')&&(dete[2]=='r')){
+            LCountDown(turn);
+            
+            } 
+        } 
+}
\ No newline at end of file

--- a/zmu9250.h	Mon Dec 05 18:31:43 2016 +0000
+++ b/zmu9250.h	Tue Dec 06 06:11:54 2016 +0000
@@ -1,24 +1,30 @@
+#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");
+              //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
-              if (whoami == 0x73) // WHO_AM_I should always be 0x68
+              aa.printf("whoami = %d\n\r ",whoami);
+              if ((whoami == 0x71))//||(whoami == 0x71)||(true)) // WHO_AM_I should always be 0x68
               {  
                 wait(1);
                 this->mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
@@ -32,14 +38,14 @@
                }
                else
                {
-                aa.printf("forever\n");
+                //aa.printf("forever\n");
                 while(1) ; // Loop forever if communication doesn't happen
                 }
-                aa.printf("first\n");
+                //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");
+                //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
@@ -48,8 +54,9 @@
                 magbias[2] = +125;  // User environmental x-axis correction in milliGauss
         }
         
-        void Update()
+        void ZMU9250::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
@@ -64,12 +71,12 @@
                 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");
+                //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",mx,my,mz);
+                //aa.printf("eleven\n\r");
+                //aa.printf("x %f\ty %f\tz %f\n\r",gyroCount[0],gy,gz);
                 
                 
             } // end if one
@@ -79,14 +86,16 @@
             this->sum += deltat;
             sumCount++;
             // Pass gyro rate as rad/s
-            if((rand()%20)>=0)
+            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);
             }else
             {
             //this->mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
             this->mpu9250.Mad_Update(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
-            }
+            }*/
             
             
             // Serial print and/or display at 0.5 s rate independent of data rates
@@ -128,17 +137,17 @@
         }
         
         
-        float Roll()
+        float ZMU9250::Roll()
         {
           return roll_x;   
         }
         
-        float Pitch()
+        float ZMU9250::Pitch()
         {
           return pitch_y;   
         }
         
-        float Yaw()
+        float ZMU9250::Yaw()
         {
           return yaw_z;   
         }
@@ -149,6 +158,7 @@
         uint32_t sumCount;
         char buffer[14];
         int roll_x;
+        kalman kal();
         int pitch_y;
         int yaw_z;
         MPU9250 mpu9250;
@@ -158,3 +168,4 @@
 };
 
 
+