Diff: main.cpp

Revision:

0:409d28bad15e

Child:

1:d2f503ea9800

diff -r 000000000000 -r 409d28bad15e main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Nov 01 20:53:03 2015 +0000
@@ -0,0 +1,74 @@
+#include "mbed.h"
+#include "MMA8451Q.h"
+
+#if defined (TARGET_KL05Z)
+PinName const SDA = PTB4;
+PinName const SCL = PTB3;
+#else
+#error TARGET NOT DEFINED
+#endif
+
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+
+int main(void)
+{
+    float y, z;
+    float val = 0.00;
+
+    PwmOut en(PTB7); // PWM for enable signal
+    DigitalOut m1(PTB6);
+    DigitalOut m2(PTA12);
+
+    MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
+
+    PwmOut rled(LED1);
+    //PwmOut gled(LED2);
+    PwmOut bled(LED3);
+
+
+    printf("MMA8451 ID: %d\n", acc.getWhoAmI());
+
+    while (true) {
+
+        /*
+        rled = 1.0f - x;
+        gled = 1.0f - y;
+        */
+        //bled = 1.0f - z;
+
+        // 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) {
+            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) {
+            m1 = 1;
+            m2 = 1;             // lock the motors
+            rled = bled = 1.0f; // turn off both leds
+        } 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);
+
+        // Write control value
+        en.write(val);
+
+        // Wait Ts
+        wait(0.1f);
+    }
+}