![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
basic functions for inverted pendulum
Dependencies: MMA8451Q-configurable MMA845x mbed
main.cpp@1:d2f503ea9800, 2015-11-19 (annotated)
- Committer:
- angusg
- Date:
- Thu Nov 19 02:13:23 2015 +0000
- Revision:
- 1:d2f503ea9800
- Parent:
- 0:409d28bad15e
- Child:
- 2:342aa0ce937e
basic inverted pendulum functionality
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 | 1:d2f503ea9800 | 3 | //#include "MMA845x.h" |
angusg | 0:409d28bad15e | 4 | |
angusg | 0:409d28bad15e | 5 | #if defined (TARGET_KL05Z) |
angusg | 0:409d28bad15e | 6 | PinName const SDA = PTB4; |
angusg | 0:409d28bad15e | 7 | PinName const SCL = PTB3; |
angusg | 0:409d28bad15e | 8 | #else |
angusg | 0:409d28bad15e | 9 | #error TARGET NOT DEFINED |
angusg | 0:409d28bad15e | 10 | #endif |
angusg | 0:409d28bad15e | 11 | |
angusg | 0:409d28bad15e | 12 | #define MMA8451_I2C_ADDRESS (0x1d<<1) |
angusg | 0:409d28bad15e | 13 | |
angusg | 1:d2f503ea9800 | 14 | //#define LOW_POWER |
angusg | 1:d2f503ea9800 | 15 | #define DEBUG |
angusg | 1:d2f503ea9800 | 16 | |
angusg | 0:409d28bad15e | 17 | int main(void) |
angusg | 0:409d28bad15e | 18 | { |
angusg | 1:d2f503ea9800 | 19 | float x, y, z = 0.00; /* Lastest X, Y, Z vals */ |
angusg | 1:d2f503ea9800 | 20 | float px, py, pz = 0.00; /* Last cycles X, Y, Z vals */ |
angusg | 1:d2f503ea9800 | 21 | float dx, dy, dz = 0.00; /* Difference */ |
angusg | 1:d2f503ea9800 | 22 | float G = 0.0; |
angusg | 1:d2f503ea9800 | 23 | float extra = 0.0; |
angusg | 1:d2f503ea9800 | 24 | |
angusg | 1:d2f503ea9800 | 25 | float ctrl_val = 0.00; /* Basically controller output U, includes sign */ |
angusg | 1:d2f503ea9800 | 26 | float ctrl_mag = 0.00; /* Magnitude of controller output U */ |
angusg | 1:d2f503ea9800 | 27 | |
angusg | 1:d2f503ea9800 | 28 | float Kp = 2.5; |
angusg | 1:d2f503ea9800 | 29 | float Kd = 0.1; |
angusg | 1:d2f503ea9800 | 30 | |
angusg | 1:d2f503ea9800 | 31 | char c; |
angusg | 1:d2f503ea9800 | 32 | |
angusg | 1:d2f503ea9800 | 33 | uint8_t data = 0; |
angusg | 0:409d28bad15e | 34 | |
angusg | 0:409d28bad15e | 35 | PwmOut en(PTB7); // PWM for enable signal |
angusg | 0:409d28bad15e | 36 | DigitalOut m1(PTB6); |
angusg | 0:409d28bad15e | 37 | DigitalOut m2(PTA12); |
angusg | 0:409d28bad15e | 38 | |
angusg | 0:409d28bad15e | 39 | MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS); |
angusg | 1:d2f503ea9800 | 40 | |
angusg | 1:d2f503ea9800 | 41 | #ifndef LOW_POWER |
angusg | 0:409d28bad15e | 42 | PwmOut rled(LED1); |
angusg | 1:d2f503ea9800 | 43 | rled = 1.0f; |
angusg | 1:d2f503ea9800 | 44 | printf("MMA8451 ID: 0x%02X\n", acc.getWhoAmI()); |
angusg | 1:d2f503ea9800 | 45 | #endif |
angusg | 1:d2f503ea9800 | 46 | |
angusg | 1:d2f503ea9800 | 47 | #ifdef DEBUG |
angusg | 1:d2f503ea9800 | 48 | for(int i=0; i<0x80; i++) |
angusg | 1:d2f503ea9800 | 49 | { //int addr, uint8_t * data, int len) |
angusg | 1:d2f503ea9800 | 50 | acc.readRegs(i, &data, 1); |
angusg | 1:d2f503ea9800 | 51 | printf("Reg 0x%02x: 0x%02x \r\n", i, data); |
angusg | 1:d2f503ea9800 | 52 | } |
angusg | 1:d2f503ea9800 | 53 | #endif |
angusg | 0:409d28bad15e | 54 | |
angusg | 1:d2f503ea9800 | 55 | c = getchar(); |
angusg | 0:409d28bad15e | 56 | |
angusg | 0:409d28bad15e | 57 | while (true) { |
angusg | 1:d2f503ea9800 | 58 | |
angusg | 1:d2f503ea9800 | 59 | /* Read accel value */ |
angusg | 1:d2f503ea9800 | 60 | x = acc.getAccX(); |
angusg | 1:d2f503ea9800 | 61 | y = acc.getAccY(); |
angusg | 1:d2f503ea9800 | 62 | z = acc.getAccZ(); |
angusg | 1:d2f503ea9800 | 63 | |
angusg | 1:d2f503ea9800 | 64 | G = sqrt( (x*x) + (y*y) + (z*z) ); |
angusg | 1:d2f503ea9800 | 65 | extra = 3 * abs(G - 1.0f); |
angusg | 1:d2f503ea9800 | 66 | |
angusg | 1:d2f503ea9800 | 67 | z = z / ( 1 + extra); // z = z divided by 1 + twice the excess G |
angusg | 1:d2f503ea9800 | 68 | //rled = 1.0f - extra/2; |
angusg | 1:d2f503ea9800 | 69 | |
angusg | 1:d2f503ea9800 | 70 | /* |
angusg | 1:d2f503ea9800 | 71 | if(z > 0.3) { |
angusg | 1:d2f503ea9800 | 72 | z = dz + pz; // Get it from the slope formula d' = ( e(n) - e(n-1) )/ Ts, but Ts = 1 cycle |
angusg | 1:d2f503ea9800 | 73 | } |
angusg | 1:d2f503ea9800 | 74 | */ |
angusg | 1:d2f503ea9800 | 75 | |
angusg | 1:d2f503ea9800 | 76 | dx = x - px; /* Get delta-x value */ |
angusg | 1:d2f503ea9800 | 77 | dy = y - py; /* Get delta-y value */ |
angusg | 1:d2f503ea9800 | 78 | dz = z - pz; /* Get delta-z value */ |
angusg | 1:d2f503ea9800 | 79 | |
angusg | 1:d2f503ea9800 | 80 | /* Y should not respond to vibrations in the Z direction */ |
angusg | 1:d2f503ea9800 | 81 | /* Consider 0.25 more than the maximum rate of falling, it must be a vibration */ |
angusg | 0:409d28bad15e | 82 | /* |
angusg | 1:d2f503ea9800 | 83 | if( (abs(dy) < 0.05) && (abs(dz) > 0.25) ){ |
angusg | 1:d2f503ea9800 | 84 | z = dz = 0; // Just do nothing, stay put |
angusg | 1:d2f503ea9800 | 85 | } |
angusg | 0:409d28bad15e | 86 | */ |
angusg | 1:d2f503ea9800 | 87 | |
angusg | 1:d2f503ea9800 | 88 | px = x; /* Update prv x */ |
angusg | 1:d2f503ea9800 | 89 | py = y; /* Update prv y */ |
angusg | 1:d2f503ea9800 | 90 | pz = z; /* Update prv z */ |
angusg | 1:d2f503ea9800 | 91 | |
angusg | 1:d2f503ea9800 | 92 | |
angusg | 1:d2f503ea9800 | 93 | ctrl_val = ( Kp * z ) + ( Kd * dz ); |
angusg | 1:d2f503ea9800 | 94 | |
angusg | 1:d2f503ea9800 | 95 | ctrl_mag = abs(ctrl_val); |
angusg | 0:409d28bad15e | 96 | |
angusg | 1:d2f503ea9800 | 97 | if(ctrl_val > 0) { |
angusg | 0:409d28bad15e | 98 | m1 = 0; |
angusg | 0:409d28bad15e | 99 | m2 = 1; |
angusg | 1:d2f503ea9800 | 100 | #ifndef LOW_POWER |
angusg | 1:d2f503ea9800 | 101 | rled = 1.0f - ctrl_mag; // make the red led proportional to the positive error signal |
angusg | 1:d2f503ea9800 | 102 | #endif |
angusg | 1:d2f503ea9800 | 103 | |
angusg | 1:d2f503ea9800 | 104 | } else if (0 == ctrl_val) { |
angusg | 0:409d28bad15e | 105 | m1 = 1; |
angusg | 0:409d28bad15e | 106 | m2 = 1; // lock the motors |
angusg | 1:d2f503ea9800 | 107 | #ifndef LOW_POWER |
angusg | 1:d2f503ea9800 | 108 | rled = 1.0f; // turn off both leds |
angusg | 1:d2f503ea9800 | 109 | #endif |
angusg | 0:409d28bad15e | 110 | } else { |
angusg | 0:409d28bad15e | 111 | m1 = 1; |
angusg | 0:409d28bad15e | 112 | m2 = 0; |
angusg | 1:d2f503ea9800 | 113 | //rled = 1.0f - ctrl_mag; // make the blue led proportional to the negative error signal |
angusg | 1:d2f503ea9800 | 114 | } |
angusg | 1:d2f503ea9800 | 115 | // x y z dZ PWM |
angusg | 1:d2f503ea9800 | 116 | printf("%1.2f,%1.2f,%1.2f,%1.2f,%1.2f\r\n", x, y, z, dz, ctrl_val); |
angusg | 0:409d28bad15e | 117 | |
angusg | 1:d2f503ea9800 | 118 | // Write control value (as percentage) |
angusg | 1:d2f503ea9800 | 119 | en.write(ctrl_mag); |
angusg | 0:409d28bad15e | 120 | |
angusg | 0:409d28bad15e | 121 | // Wait Ts |
angusg | 1:d2f503ea9800 | 122 | wait_ms(75); |
angusg | 0:409d28bad15e | 123 | } |
angusg | 0:409d28bad15e | 124 | } |