Angus Galloway / Mbed 2 deprecated KL05_accel-test

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?

UserRevisionLine numberNew 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 }