basic functions for inverted pendulum
Dependencies: MMA8451Q-configurable MMA845x mbed
Diff: main.cpp
- Revision:
- 1:d2f503ea9800
- Parent:
- 0:409d28bad15e
- Child:
- 2:342aa0ce937e
diff -r 409d28bad15e -r d2f503ea9800 main.cpp --- a/main.cpp Sun Nov 01 20:53:03 2015 +0000 +++ b/main.cpp Thu Nov 19 02:13:23 2015 +0000 @@ -1,5 +1,6 @@ #include "mbed.h" #include "MMA8451Q.h" +//#include "MMA845x.h" #if defined (TARGET_KL05Z) PinName const SDA = PTB4; @@ -10,65 +11,114 @@ #define MMA8451_I2C_ADDRESS (0x1d<<1) +//#define LOW_POWER +#define DEBUG + int main(void) { - float y, z; - float val = 0.00; + float x, y, z = 0.00; /* Lastest X, Y, Z vals */ + float px, py, pz = 0.00; /* Last cycles X, Y, Z vals */ + float dx, dy, dz = 0.00; /* Difference */ + float G = 0.0; + float extra = 0.0; + + float ctrl_val = 0.00; /* Basically controller output U, includes sign */ + float ctrl_mag = 0.00; /* Magnitude of controller output U */ + + float Kp = 2.5; + float Kd = 0.1; + + char c; + + uint8_t data = 0; PwmOut en(PTB7); // PWM for enable signal DigitalOut m1(PTB6); DigitalOut m2(PTA12); MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS); - + +#ifndef LOW_POWER PwmOut rled(LED1); - //PwmOut gled(LED2); - PwmOut bled(LED3); + rled = 1.0f; + printf("MMA8451 ID: 0x%02X\n", acc.getWhoAmI()); +#endif + +#ifdef DEBUG + for(int i=0; i<0x80; i++) + { //int addr, uint8_t * data, int len) + acc.readRegs(i, &data, 1); + printf("Reg 0x%02x: 0x%02x \r\n", i, data); + } +#endif - - printf("MMA8451 ID: %d\n", acc.getWhoAmI()); + c = getchar(); while (true) { - + + /* Read accel value */ + x = acc.getAccX(); + y = acc.getAccY(); + z = acc.getAccZ(); + + G = sqrt( (x*x) + (y*y) + (z*z) ); + extra = 3 * abs(G - 1.0f); + + z = z / ( 1 + extra); // z = z divided by 1 + twice the excess G + //rled = 1.0f - extra/2; + + /* + if(z > 0.3) { + z = dz + pz; // Get it from the slope formula d' = ( e(n) - e(n-1) )/ Ts, but Ts = 1 cycle + } + */ + + dx = x - px; /* Get delta-x value */ + dy = y - py; /* Get delta-y value */ + dz = z - pz; /* Get delta-z value */ + + /* Y should not respond to vibrations in the Z direction */ + /* Consider 0.25 more than the maximum rate of falling, it must be a vibration */ /* - rled = 1.0f - x; - gled = 1.0f - y; + if( (abs(dy) < 0.05) && (abs(dz) > 0.25) ){ + z = dz = 0; // Just do nothing, stay put + } */ - //bled = 1.0f - z; + + px = x; /* Update prv x */ + py = y; /* Update prv y */ + pz = z; /* Update prv z */ + + + ctrl_val = ( Kp * z ) + ( Kd * dz ); + + ctrl_mag = abs(ctrl_val); - // 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) { + if(ctrl_val > 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) { +#ifndef LOW_POWER + rled = 1.0f - ctrl_mag; // make the red led proportional to the positive error signal +#endif + + } else if (0 == ctrl_val) { m1 = 1; m2 = 1; // lock the motors - rled = bled = 1.0f; // turn off both leds +#ifndef LOW_POWER + rled = 1.0f; // turn off both leds +#endif } 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); + //rled = 1.0f - ctrl_mag; // make the blue led proportional to the negative error signal + } + // x y z dZ PWM + printf("%1.2f,%1.2f,%1.2f,%1.2f,%1.2f\r\n", x, y, z, dz, ctrl_val); - // Write control value - en.write(val); + // Write control value (as percentage) + en.write(ctrl_mag); // Wait Ts - wait(0.1f); + wait_ms(75); } }