Team Alpha / Mbed 2 deprecated UserIntefaceLCD

LCD implementation of our project.

Dependencies:   mbed mbed-rtos MLX90614

gyro.cpp@6:49a007861c76, 2015-05-30 (annotated)

Committer:
ovidiup13
Date:
Sat May 30 14:58:44 2015 +0000
Revision:
6:49a007861c76
Child:
8:81ed1135ba02
integrated level meter functionality and added 3-way switch control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ovidiup13 6:49a007861c76 1 #include "gyro.h"
ovidiup13 6:49a007861c76 2
ovidiup13 6:49a007861c76 3 //DigitalOut myled(LED1);
ovidiup13 6:49a007861c76 4 //Serial pc(USBTX, USBRX); // tx, rx for USB debug printf to terminal console
ovidiup13 6:49a007861c76 5 I2C i2c(p28, p27); // LPC1768 I2C pin allocation
ovidiup13 6:49a007861c76 6 //DigitalIn din(p23); // used as a test button
ovidiup13 6:49a007861c76 7
ovidiup13 6:49a007861c76 8 // Globals
ovidiup13 6:49a007861c76 9 int16_t const Offset_mX=-40.0;
ovidiup13 6:49a007861c76 10 int16_t const Offset_mY=-115.0;
ovidiup13 6:49a007861c76 11 float const RadtoDeg=(180.0/3.141592654);
ovidiup13 6:49a007861c76 12
ovidiup13 6:49a007861c76 13
ovidiup13 6:49a007861c76 14 char readByte(char address, char reg)
ovidiup13 6:49a007861c76 15 // Reads one byte from an I2C address
ovidiup13 6:49a007861c76 16 // Didn't bother to make a multi-byte version to read in X,Y,Z low/high series of registers because...
ovidiup13 6:49a007861c76 17 // the data registers of all sensors they are in the same XL,XH,YL,YH,ZL,ZH order apart from the magnetometer which is XH,XL,ZH,ZL,YH,YL...
ovidiup13 6:49a007861c76 18 {
ovidiup13 6:49a007861c76 19 char result;
ovidiup13 6:49a007861c76 20
ovidiup13 6:49a007861c76 21 i2c.start();
ovidiup13 6:49a007861c76 22 i2c.write(address); // Slave address with direction=write
ovidiup13 6:49a007861c76 23 i2c.write(reg); // Subaddress (register)
ovidiup13 6:49a007861c76 24
ovidiup13 6:49a007861c76 25 i2c.start(); // Break transmission to change bus direction
ovidiup13 6:49a007861c76 26 i2c.write(address + 1); // Slave address with direction=read [bit0=1]
ovidiup13 6:49a007861c76 27
ovidiup13 6:49a007861c76 28 result = i2c.read(0);
ovidiup13 6:49a007861c76 29 i2c.stop();
ovidiup13 6:49a007861c76 30 return (result);
ovidiup13 6:49a007861c76 31 }
ovidiup13 6:49a007861c76 32
ovidiup13 6:49a007861c76 33 void writeByte(char address, char reg,char value)
ovidiup13 6:49a007861c76 34 // Sends 1 byte to an I2C address
ovidiup13 6:49a007861c76 35 {
ovidiup13 6:49a007861c76 36 i2c.start();
ovidiup13 6:49a007861c76 37 i2c.write(address); // Slave address
ovidiup13 6:49a007861c76 38 i2c.write(reg); // Subaddress (register)
ovidiup13 6:49a007861c76 39 i2c.write(value);
ovidiup13 6:49a007861c76 40 i2c.stop();
ovidiup13 6:49a007861c76 41
ovidiup13 6:49a007861c76 42 }
ovidiup13 6:49a007861c76 43
ovidiup13 6:49a007861c76 44 void initSensors (void)
ovidiup13 6:49a007861c76 45 // Switch on and set up the 3 on-board sensors
ovidiup13 6:49a007861c76 46 {
ovidiup13 6:49a007861c76 47 //pc.printf("--------------------------------------\n");
ovidiup13 6:49a007861c76 48 //pc.printf("\nSTM MEMS eval board sensor init \n");
ovidiup13 6:49a007861c76 49
ovidiup13 6:49a007861c76 50 #ifdef LSM303_on
ovidiup13 6:49a007861c76 51 // LSM303DLHC Magnetic sensor
ovidiup13 6:49a007861c76 52 //pc.printf("LSM303DLHC ping (should reply 0x48): %x \n",readByte(LSM303_m,mIRA_REG_M));
ovidiup13 6:49a007861c76 53 writeByte(LSM303_m,mCRA_REG_M,0x94); //switch on temperature sensor and set Output Data Rate to 30Hz
ovidiup13 6:49a007861c76 54 writeByte(LSM303_m,mCRB_REG_M,0x20); // Set the gain for +/- 1.3 Gauss full scale range
ovidiup13 6:49a007861c76 55 writeByte(LSM303_m,mMR_REG_M,0x0); // Continuous convertion mode
ovidiup13 6:49a007861c76 56
ovidiup13 6:49a007861c76 57 // LSM303DLHC Accelerometer
ovidiup13 6:49a007861c76 58 writeByte(LSM303_a,aCTRL_REG1_A ,0x37); // Set 25Hz ODR, everything else on
ovidiup13 6:49a007861c76 59 writeByte(LSM303_a,aCTRL_REG4_A ,0x08); // Set full scale to +/- 2g sensitivity and high rez mode
ovidiup13 6:49a007861c76 60 #endif
ovidiup13 6:49a007861c76 61
ovidiup13 6:49a007861c76 62 //pc.printf("--------------------------------------\n \n");
ovidiup13 6:49a007861c76 63 wait(2); // Wait for settings to stabilize
ovidiup13 6:49a007861c76 64 }
ovidiup13 6:49a007861c76 65
ovidiup13 6:49a007861c76 66 void LSM303 (SensorState_t * state)
ovidiup13 6:49a007861c76 67 // Magnetometer and accelerometer
ovidiup13 6:49a007861c76 68 {
ovidiup13 6:49a007861c76 69 char xL, xH, yL, yH, zL, zH;
ovidiup13 6:49a007861c76 70 int16_t mX, mY, mZ,aX,aY,aZ;
ovidiup13 6:49a007861c76 71 float pitch,roll,faX,faY;
ovidiup13 6:49a007861c76 72
ovidiup13 6:49a007861c76 73 xL=readByte(LSM303_m,mOUT_X_L_M);
ovidiup13 6:49a007861c76 74 xH=readByte(LSM303_m,mOUT_X_H_M);
ovidiup13 6:49a007861c76 75 yL=readByte(LSM303_m,mOUT_Y_L_M);
ovidiup13 6:49a007861c76 76 yH=readByte(LSM303_m,mOUT_Y_H_M);
ovidiup13 6:49a007861c76 77 zL=readByte(LSM303_m,mOUT_Z_L_M);
ovidiup13 6:49a007861c76 78 zH=readByte(LSM303_m,mOUT_Z_H_M);
ovidiup13 6:49a007861c76 79
ovidiup13 6:49a007861c76 80 mX=(xH<<8) | (xL); // 16-bit 2's complement data
ovidiup13 6:49a007861c76 81 mY=(yH<<8) | (yL);
ovidiup13 6:49a007861c76 82 mZ=(zH<<8) | (zL);
ovidiup13 6:49a007861c76 83
ovidiup13 6:49a007861c76 84 //pc.printf("mX=%hd %X mY=%hd %X mZ=%hd %X \n",mX,mX,mY,mY,mZ,mZ);
ovidiup13 6:49a007861c76 85
ovidiup13 6:49a007861c76 86 mX=mX-Offset_mX; // These are callibration co-efficients to deal with non-zero soft iron magnetic offset
ovidiup13 6:49a007861c76 87 mY=mY-Offset_mY;
ovidiup13 6:49a007861c76 88
ovidiup13 6:49a007861c76 89 xL=readByte(LSM303_a,aOUT_X_L_A);
ovidiup13 6:49a007861c76 90 xH=readByte(LSM303_a,aOUT_X_H_A);
ovidiup13 6:49a007861c76 91 yL=readByte(LSM303_a,aOUT_Y_L_A);
ovidiup13 6:49a007861c76 92 yH=readByte(LSM303_a,aOUT_Y_H_A);
ovidiup13 6:49a007861c76 93 zL=readByte(LSM303_a,aOUT_Z_L_A);
ovidiup13 6:49a007861c76 94 zH=readByte(LSM303_a,aOUT_Z_H_A);
ovidiup13 6:49a007861c76 95
ovidiup13 6:49a007861c76 96 aX=(signed short) ( (xH<<8) | (xL) ) >> 4; // 12-bit data from ADC. Cast ensures that the 2's complement sign is not lost in the right shift.
ovidiup13 6:49a007861c76 97 aY=(signed short) ( (yH<<8) | (yL) ) >> 4;
ovidiup13 6:49a007861c76 98 aZ=(signed short) ( (zH<<8) | (zL) ) >> 4;
ovidiup13 6:49a007861c76 99
ovidiup13 6:49a007861c76 100 //pc.printf("aX=%hd %X aY=%hd %X aZ=%hd %X \n",aX,aX,aY,aY,aZ,aZ);
ovidiup13 6:49a007861c76 101
ovidiup13 6:49a007861c76 102 faX=((float) aX) /2000.0; // Accelerometer scale I chose is 1mg per LSB with range +/-2g. So to normalize for full scale need to divide by 2000.
ovidiup13 6:49a007861c76 103 faY=((float) aY) /2000.0; // If you don't do this the pitch and roll calcs will not work (inverse cosine of a value greater than 1)
ovidiup13 6:49a007861c76 104 //faZ=((float) aZ) /2000.0; // Not used in a calc so comment out to avoid the compiler warning
ovidiup13 6:49a007861c76 105
ovidiup13 6:49a007861c76 106 // Trigonometry derived from STM app note AN3192 and from WikiRobots
ovidiup13 6:49a007861c76 107 pitch = asin((float) -faX*2); // Dividing faX and faY by 1000 rather than 2000 seems to give better tilt immunity. Do it here rather than above to preserve true mg units of faX etc
ovidiup13 6:49a007861c76 108 roll = asin(faY*2/cos(pitch));
ovidiup13 6:49a007861c76 109
ovidiup13 6:49a007861c76 110 float xh = mX * cos(pitch) + mZ * sin(pitch);
ovidiup13 6:49a007861c76 111 float yh = mX * sin(roll) * sin(pitch) + mY * cos(roll) - mZ * sin(roll) * cos(pitch);
ovidiup13 6:49a007861c76 112 float zh = -mX * cos(roll) * sin(pitch) + mY * sin(roll) + mZ * cos(roll) * cos(pitch);
ovidiup13 6:49a007861c76 113
ovidiup13 6:49a007861c76 114 float heading = atan2(yh, xh) * RadtoDeg; // Note use of atan2 rather than atan since better for working with quadrants
ovidiup13 6:49a007861c76 115 if (yh < 0)
ovidiup13 6:49a007861c76 116 heading=360+heading;
ovidiup13 6:49a007861c76 117
ovidiup13 6:49a007861c76 118 state->heading=heading;
ovidiup13 6:49a007861c76 119 state->pitch=pitch;
ovidiup13 6:49a007861c76 120 state->roll=roll;
ovidiup13 6:49a007861c76 121 state->aX = (float)aX;
ovidiup13 6:49a007861c76 122 state->aY = (float)aY;
ovidiup13 6:49a007861c76 123 state->aZ = (float)aZ;
ovidiup13 6:49a007861c76 124 //5.1f
ovidiup13 6:49a007861c76 125 //pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n",roll*RadtoDeg,pitch*RadtoDeg,heading);
ovidiup13 6:49a007861c76 126 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",aX,aY,aZ);
ovidiup13 6:49a007861c76 127
ovidiup13 6:49a007861c76 128 }
ovidiup13 6:49a007861c76 129
ovidiup13 6:49a007861c76 130 //calculates the difference for acceleration in int16_t value
ovidiup13 6:49a007861c76 131 void calc_avrg_ac(Result_avrg* result,int samples){
ovidiup13 6:49a007861c76 132 int i = 0;
ovidiup13 6:49a007861c76 133 result -> aX = 0;
ovidiup13 6:49a007861c76 134 result -> aY = 0;
ovidiup13 6:49a007861c76 135 result -> aZ = 0;
ovidiup13 6:49a007861c76 136 SensorState_t state;
ovidiup13 6:49a007861c76 137 for(i = 0;i<samples;i++){
ovidiup13 6:49a007861c76 138 #ifdef LSM303_on
ovidiup13 6:49a007861c76 139 LSM303(&state);
ovidiup13 6:49a007861c76 140 #endif
ovidiup13 6:49a007861c76 141 result -> aX += state.aX;
ovidiup13 6:49a007861c76 142 result -> aY += state.aY;
ovidiup13 6:49a007861c76 143 result -> aZ += state.aZ;
ovidiup13 6:49a007861c76 144 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",state.aX,state.aY,state.aZ);
ovidiup13 6:49a007861c76 145
ovidiup13 6:49a007861c76 146 wait(0.01);
ovidiup13 6:49a007861c76 147 }
ovidiup13 6:49a007861c76 148 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result->aX,result->aY,result->aZ);
ovidiup13 6:49a007861c76 149 result -> aX = result -> aX / samples;
ovidiup13 6:49a007861c76 150 result -> aY = result -> aY / samples;
ovidiup13 6:49a007861c76 151 result -> aZ = result -> aZ / samples;
ovidiup13 6:49a007861c76 152 //pc.printf("rAcceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result->aX,result->aY,result->aZ);
ovidiup13 6:49a007861c76 153 }
ovidiup13 6:49a007861c76 154
ovidiup13 6:49a007861c76 155 //calculates the difference for orientation in float value
ovidiup13 6:49a007861c76 156 void calc_avrg_or(Result_avrg* result,int samples){
ovidiup13 6:49a007861c76 157 int i = 0;
ovidiup13 6:49a007861c76 158 result -> x = 0;
ovidiup13 6:49a007861c76 159 result -> y = 0;
ovidiup13 6:49a007861c76 160 result -> z = 0;
ovidiup13 6:49a007861c76 161 SensorState_t state;
ovidiup13 6:49a007861c76 162 for(i = 0;i<samples;i++){
ovidiup13 6:49a007861c76 163 #ifdef LSM303_on
ovidiup13 6:49a007861c76 164 LSM303(&state);
ovidiup13 6:49a007861c76 165 #endif
ovidiup13 6:49a007861c76 166 result -> x += state.roll*RadtoDeg;
ovidiup13 6:49a007861c76 167 result -> y += state.pitch*RadtoDeg;
ovidiup13 6:49a007861c76 168 result -> z += state.heading;
ovidiup13 6:49a007861c76 169 //pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n",state.roll*RadtoDeg,state.pitch*RadtoDeg,state.heading);
ovidiup13 6:49a007861c76 170 wait(0.01);
ovidiup13 6:49a007861c76 171 }
ovidiup13 6:49a007861c76 172 //pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n", result -> x ,result -> y,result -> z);
ovidiup13 6:49a007861c76 173 result -> x = result -> x / samples;
ovidiup13 6:49a007861c76 174 result -> y = result -> y / samples;
ovidiup13 6:49a007861c76 175 result -> z = result -> z / samples;
ovidiup13 6:49a007861c76 176 //pc.printf("Orientation (deg): rRot_X: %0.0f rRot_Y: %0.0f rRot_Z: %0.0f \n", result -> x ,result -> y,result -> z);
ovidiup13 6:49a007861c76 177 }
ovidiup13 6:49a007861c76 178
ovidiup13 6:49a007861c76 179 //gets the two results and saves the answer in r1 structure
ovidiup13 6:49a007861c76 180 void calc_diff(Result_avrg* r1, Result_avrg* r2){
ovidiup13 6:49a007861c76 181 r1 -> x = abs(r1->x - r2->x);
ovidiup13 6:49a007861c76 182 r1 -> y = abs(r1->y - r2->y);
ovidiup13 6:49a007861c76 183 r1 -> z = abs(r1->z - r2->z);
ovidiup13 6:49a007861c76 184
ovidiup13 6:49a007861c76 185 r1 -> aX = abs(r1->aX - r2->aX);
ovidiup13 6:49a007861c76 186 r1 -> aY = abs(r1->aY - r2->aY);
ovidiup13 6:49a007861c76 187 r1 -> aZ = abs(r1->aZ - r2->aZ);
ovidiup13 6:49a007861c76 188 }
ovidiup13 6:49a007861c76 189
ovidiup13 6:49a007861c76 190 /*
ovidiup13 6:49a007861c76 191
ovidiup13 6:49a007861c76 192 int main()
ovidiup13 6:49a007861c76 193 {
ovidiup13 6:49a007861c76 194 SensorState_t state;
ovidiup13 6:49a007861c76 195 Result_avrg result1;
ovidiup13 6:49a007861c76 196 //Result_avrg result2;
ovidiup13 6:49a007861c76 197 initSensors();
ovidiup13 6:49a007861c76 198 //pc.baud(115200);
ovidiup13 6:49a007861c76 199 calc_avrg_or(&result1,3);
ovidiup13 6:49a007861c76 200 //calc_avrg_ac(&result1,3);
ovidiup13 6:49a007861c76 201 //pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n", result1.x ,result1.y,result1.z);
ovidiup13 6:49a007861c76 202 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result1.aX,result1.aY,result1.aZ);
ovidiup13 6:49a007861c76 203 //calc_avrg_or(&result2,3);
ovidiup13 6:49a007861c76 204 //calc_avrg_ac(&result2,3);
ovidiup13 6:49a007861c76 205 //pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n", result2.x ,result2.y,result2.z);
ovidiup13 6:49a007861c76 206 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result2.aX,result2.aY,result2.aZ);
ovidiup13 6:49a007861c76 207 //calc_diff(&result1,&result2);
ovidiup13 6:49a007861c76 208 //pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n", result1.x ,result1.y,result1.z);
ovidiup13 6:49a007861c76 209 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result1.aX,result1.aY,result1.aZ);
ovidiup13 6:49a007861c76 210 #ifdef LSM303_on
ovidiup13 6:49a007861c76 211 //LSM303(&state);
ovidiup13 6:49a007861c76 212 #endif
ovidiup13 6:49a007861c76 213
ovidiup13 6:49a007861c76 214 //pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n",state.roll*RadtoDeg,state.pitch*RadtoDeg,state.heading);
ovidiup13 6:49a007861c76 215 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",state.aX,state.aY,state.aZ);
ovidiup13 6:49a007861c76 216 //pc.printf("\n");
ovidiup13 6:49a007861c76 217
ovidiup13 6:49a007861c76 218 }
ovidiup13 6:49a007861c76 219 */