Diff: main.cpp

Revision:

1:d2f503ea9800

Parent:

0:409d28bad15e

Child:

2:342aa0ce937e

--- a/main.cpp	Sun Nov 01 20:53:03 2015 +0000
+++ b/main.cpp	Thu Nov 19 02:13:23 2015 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "MMA8451Q.h"
+//#include "MMA845x.h"
 
 #if defined (TARGET_KL05Z)
 PinName const SDA = PTB4;
@@ -10,65 +11,114 @@
 
 #define MMA8451_I2C_ADDRESS (0x1d<<1)
 
+//#define LOW_POWER
+#define DEBUG
+
 int main(void)
 {
-    float y, z;
-    float val = 0.00;
+    float x, y, z = 0.00;        /* Lastest X, Y, Z vals */
+    float px, py, pz = 0.00;     /* Last cycles X, Y, Z vals */
+    float dx, dy, dz = 0.00;     /* Difference */
+    float G = 0.0;
+    float extra = 0.0;
+    
+    float ctrl_val = 0.00;          /* Basically controller output U, includes sign */
+    float ctrl_mag = 0.00;          /* Magnitude of controller output U */
+        
+    float Kp = 2.5;
+    float Kd = 0.1;
+    
+    char c;
+    
+    uint8_t data = 0;
 
     PwmOut en(PTB7); // PWM for enable signal
     DigitalOut m1(PTB6);
     DigitalOut m2(PTA12);
 
     MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
-
+    
+#ifndef LOW_POWER
     PwmOut rled(LED1);
-    //PwmOut gled(LED2);
-    PwmOut bled(LED3);
+    rled = 1.0f;
+    printf("MMA8451 ID: 0x%02X\n", acc.getWhoAmI());
+#endif    
+    
+#ifdef DEBUG
+    for(int i=0; i<0x80; i++)
+    {                                //int addr, uint8_t * data, int len)
+        acc.readRegs(i, &data, 1);
+        printf("Reg 0x%02x: 0x%02x \r\n", i, data);
+    }
+#endif
 
-
-    printf("MMA8451 ID: %d\n", acc.getWhoAmI());
+    c = getchar();
 
     while (true) {
-
+        
+        /* Read accel value */        
+        x = acc.getAccX();  
+        y = acc.getAccY();
+        z = acc.getAccZ();    
+        
+        G = sqrt( (x*x) + (y*y) + (z*z) );
+        extra = 3 * abs(G - 1.0f);
+        
+        z = z / ( 1 +  extra); // z = z divided by 1 + twice the excess G
+        //rled = 1.0f - extra/2;             
+        
+        /*     
+        if(z > 0.3) {
+            z = dz + pz;       // Get it from the slope formula d' = ( e(n) - e(n-1) )/ Ts, but Ts = 1 cycle
+        }
+        */
+        
+        dx = x - px;    /* Get delta-x value */
+        dy = y - py;    /* Get delta-y value */
+        dz = z - pz;    /* Get delta-z value */               
+        
+        /* Y should not respond to vibrations in the Z direction */
+        /* Consider 0.25 more than the maximum rate of falling, it must be a vibration */
         /*
-        rled = 1.0f - x;
-        gled = 1.0f - y;
+        if( (abs(dy) < 0.05) && (abs(dz) > 0.25) ){
+            z = dz = 0; // Just do nothing, stay put
+        }
         */
-        //bled = 1.0f - z;
+                  
+        px = x;                /* Update prv x */ 
+        py = y;                /* Update prv y */ 
+        pz = z;                /* Update prv z */  
+          
+              
+        ctrl_val = ( Kp * z ) + ( Kd * dz );
+        
+        ctrl_mag = abs(ctrl_val);
 
-        // Calculate next value (h computational delay)
-        // y = acc.getAccY();
-        z = acc.getAccZ();        
-
-        val = sqrt( z * z );
-        
-        /*
-        if(val < 0.02)
-            val = 0;
-         */   
-
-        if(z > 0) {
+        if(ctrl_val > 0) {
             m1 = 0;
             m2 = 1;
-            bled = 1.0f;       // turn off blue led
-            rled = 1.0f - val; // make the red led proportional to the positive error signal
-        } else if (0 == z) {
+#ifndef LOW_POWER
+            rled = 1.0f - ctrl_mag; // make the red led proportional to the positive error signal
+#endif                 
+ 
+        } else if (0 == ctrl_val) {
             m1 = 1;
             m2 = 1;             // lock the motors
-            rled = bled = 1.0f; // turn off both leds
+#ifndef LOW_POWER
+            rled = 1.0f; // turn off both leds
+#endif                 
         } else {
             m1 = 1;
             m2 = 0;
-            rled = 1.0f;       // turn off red led
-            bled = 1.0f - val; // make the blue led proportional to the negative error signal
-        }
-        
-        printf("Z: %1.2f, PWM: %1.2f\r\n", z, val);
+            //rled = 1.0f - ctrl_mag; // make the blue led proportional to the negative error signal
+        }             
+        //      x     y     z     dZ    PWM
+        printf("%1.2f,%1.2f,%1.2f,%1.2f,%1.2f\r\n", x, y, z, dz, ctrl_val);      
 
-        // Write control value
-        en.write(val);
+        // Write control value (as percentage)
+        en.write(ctrl_mag);
 
         // Wait Ts
-        wait(0.1f);
+        wait_ms(75);
     }
 }