![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
basic functions for inverted pendulum
Dependencies: MMA8451Q-configurable MMA845x mbed
main.cpp
- Committer:
- angusg
- Date:
- 2015-11-01
- Revision:
- 0:409d28bad15e
- Child:
- 1:d2f503ea9800
File content as of revision 0:409d28bad15e:
#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); } }