![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
basic functions for inverted pendulum
Dependencies: MMA8451Q-configurable MMA845x mbed
main.cpp@0:409d28bad15e, 2015-11-01 (annotated)
- Committer:
- angusg
- Date:
- Sun Nov 01 20:53:03 2015 +0000
- Revision:
- 0:409d28bad15e
- Child:
- 1:d2f503ea9800
Base balancing, K controller
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
angusg | 0:409d28bad15e | 1 | #include "mbed.h" |
angusg | 0:409d28bad15e | 2 | #include "MMA8451Q.h" |
angusg | 0:409d28bad15e | 3 | |
angusg | 0:409d28bad15e | 4 | #if defined (TARGET_KL05Z) |
angusg | 0:409d28bad15e | 5 | PinName const SDA = PTB4; |
angusg | 0:409d28bad15e | 6 | PinName const SCL = PTB3; |
angusg | 0:409d28bad15e | 7 | #else |
angusg | 0:409d28bad15e | 8 | #error TARGET NOT DEFINED |
angusg | 0:409d28bad15e | 9 | #endif |
angusg | 0:409d28bad15e | 10 | |
angusg | 0:409d28bad15e | 11 | #define MMA8451_I2C_ADDRESS (0x1d<<1) |
angusg | 0:409d28bad15e | 12 | |
angusg | 0:409d28bad15e | 13 | int main(void) |
angusg | 0:409d28bad15e | 14 | { |
angusg | 0:409d28bad15e | 15 | float y, z; |
angusg | 0:409d28bad15e | 16 | float val = 0.00; |
angusg | 0:409d28bad15e | 17 | |
angusg | 0:409d28bad15e | 18 | PwmOut en(PTB7); // PWM for enable signal |
angusg | 0:409d28bad15e | 19 | DigitalOut m1(PTB6); |
angusg | 0:409d28bad15e | 20 | DigitalOut m2(PTA12); |
angusg | 0:409d28bad15e | 21 | |
angusg | 0:409d28bad15e | 22 | MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS); |
angusg | 0:409d28bad15e | 23 | |
angusg | 0:409d28bad15e | 24 | PwmOut rled(LED1); |
angusg | 0:409d28bad15e | 25 | //PwmOut gled(LED2); |
angusg | 0:409d28bad15e | 26 | PwmOut bled(LED3); |
angusg | 0:409d28bad15e | 27 | |
angusg | 0:409d28bad15e | 28 | |
angusg | 0:409d28bad15e | 29 | printf("MMA8451 ID: %d\n", acc.getWhoAmI()); |
angusg | 0:409d28bad15e | 30 | |
angusg | 0:409d28bad15e | 31 | while (true) { |
angusg | 0:409d28bad15e | 32 | |
angusg | 0:409d28bad15e | 33 | /* |
angusg | 0:409d28bad15e | 34 | rled = 1.0f - x; |
angusg | 0:409d28bad15e | 35 | gled = 1.0f - y; |
angusg | 0:409d28bad15e | 36 | */ |
angusg | 0:409d28bad15e | 37 | //bled = 1.0f - z; |
angusg | 0:409d28bad15e | 38 | |
angusg | 0:409d28bad15e | 39 | // Calculate next value (h computational delay) |
angusg | 0:409d28bad15e | 40 | // y = acc.getAccY(); |
angusg | 0:409d28bad15e | 41 | z = acc.getAccZ(); |
angusg | 0:409d28bad15e | 42 | |
angusg | 0:409d28bad15e | 43 | val = sqrt( z * z ); |
angusg | 0:409d28bad15e | 44 | |
angusg | 0:409d28bad15e | 45 | /* |
angusg | 0:409d28bad15e | 46 | if(val < 0.02) |
angusg | 0:409d28bad15e | 47 | val = 0; |
angusg | 0:409d28bad15e | 48 | */ |
angusg | 0:409d28bad15e | 49 | |
angusg | 0:409d28bad15e | 50 | if(z > 0) { |
angusg | 0:409d28bad15e | 51 | m1 = 0; |
angusg | 0:409d28bad15e | 52 | m2 = 1; |
angusg | 0:409d28bad15e | 53 | bled = 1.0f; // turn off blue led |
angusg | 0:409d28bad15e | 54 | rled = 1.0f - val; // make the red led proportional to the positive error signal |
angusg | 0:409d28bad15e | 55 | } else if (0 == z) { |
angusg | 0:409d28bad15e | 56 | m1 = 1; |
angusg | 0:409d28bad15e | 57 | m2 = 1; // lock the motors |
angusg | 0:409d28bad15e | 58 | rled = bled = 1.0f; // turn off both leds |
angusg | 0:409d28bad15e | 59 | } else { |
angusg | 0:409d28bad15e | 60 | m1 = 1; |
angusg | 0:409d28bad15e | 61 | m2 = 0; |
angusg | 0:409d28bad15e | 62 | rled = 1.0f; // turn off red led |
angusg | 0:409d28bad15e | 63 | bled = 1.0f - val; // make the blue led proportional to the negative error signal |
angusg | 0:409d28bad15e | 64 | } |
angusg | 0:409d28bad15e | 65 | |
angusg | 0:409d28bad15e | 66 | printf("Z: %1.2f, PWM: %1.2f\r\n", z, val); |
angusg | 0:409d28bad15e | 67 | |
angusg | 0:409d28bad15e | 68 | // Write control value |
angusg | 0:409d28bad15e | 69 | en.write(val); |
angusg | 0:409d28bad15e | 70 | |
angusg | 0:409d28bad15e | 71 | // Wait Ts |
angusg | 0:409d28bad15e | 72 | wait(0.1f); |
angusg | 0:409d28bad15e | 73 | } |
angusg | 0:409d28bad15e | 74 | } |