basic functions for inverted pendulum
Dependencies: MMA8451Q-configurable MMA845x mbed
main.cpp
- Committer:
- angusg
- Date:
- 2015-11-19
- Revision:
- 2:342aa0ce937e
- Parent:
- 1:d2f503ea9800
File content as of revision 2:342aa0ce937e:
#include "mbed.h" #include "MMA8451Q.h" //#include "MMA845x.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) #define LOW_POWER //#define DEBUG int main(void) { 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.2; 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); rled = 1.0f; printf("MMA8451 ID: 0x%02X\n", acc.getWhoAmI()); #endif #ifdef DEBUG for(int i=0; i<0x50; i++) { //int addr, uint8_t * data, int len) acc.readRegs(i, &data, 1); printf("Reg 0x%02x: 0x%02x \r\n", i, data); } c = getchar(); #endif 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 */ /* if( (abs(dy) < 0.05) && (abs(dz) > 0.25) ){ z = dz = 0; // Just do nothing, stay put } */ 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); if(ctrl_val > 0) { m1 = 0; m2 = 1; #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 #ifndef LOW_POWER rled = 1.0f; // turn off both leds #endif } */ else { m1 = 1; m2 = 0; //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 (as percentage) en.write(ctrl_mag); // Wait Ts wait_ms(75); } }