Angus Galloway / Mbed 2 deprecated KL05_accel-test

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?

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