![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
basic functions for inverted pendulum
Dependencies: MMA8451Q-configurable MMA845x mbed
Diff: main.cpp
- Revision:
- 0:409d28bad15e
- Child:
- 1:d2f503ea9800
--- /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); + } +}