Team Alpha / Mbed 2 deprecated UserIntefaceLCD

LCD implementation of our project.

Dependencies:   mbed mbed-rtos MLX90614

gyro.cpp@10:97389d774ae1, 2015-06-03 (annotated)

Committer:
ovidiup13
Date:
Wed Jun 03 17:42:47 2015 +0000
Revision:
10:97389d774ae1
Parent:
8:81ed1135ba02 
working version, comment out thermo in main;

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 8:81ed1135ba02 5 I2C i2c(p9, p10); // 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 8:81ed1135ba02 13 MLX90614 IR_thermometer(&i2c);
ovidiup13 8:81ed1135ba02 14
ovidiup13 8:81ed1135ba02 15 float get_temperature(void){
ovidiup13 8:81ed1135ba02 16 float temp;
ovidiup13 8:81ed1135ba02 17 IR_thermometer.getTemp(&temp);
ovidiup13 8:81ed1135ba02 18 return temp;
ovidiup13 8:81ed1135ba02 19 }
ovidiup13 6:49a007861c76 20
ovidiup13 6:49a007861c76 21 char readByte(char address, char reg)
ovidiup13 6:49a007861c76 22 // Reads one byte from an I2C address
ovidiup13 6:49a007861c76 23 // Didn't bother to make a multi-byte version to read in X,Y,Z low/high series of registers because...
ovidiup13 6:49a007861c76 24 // 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 25 {
ovidiup13 6:49a007861c76 26 char result;
ovidiup13 6:49a007861c76 27
ovidiup13 6:49a007861c76 28 i2c.start();
ovidiup13 6:49a007861c76 29 i2c.write(address); // Slave address with direction=write
ovidiup13 6:49a007861c76 30 i2c.write(reg); // Subaddress (register)
ovidiup13 6:49a007861c76 31
ovidiup13 6:49a007861c76 32 i2c.start(); // Break transmission to change bus direction
ovidiup13 6:49a007861c76 33 i2c.write(address + 1); // Slave address with direction=read [bit0=1]
ovidiup13 6:49a007861c76 34
ovidiup13 6:49a007861c76 35 result = i2c.read(0);
ovidiup13 6:49a007861c76 36 i2c.stop();
ovidiup13 6:49a007861c76 37 return (result);
ovidiup13 6:49a007861c76 38 }
ovidiup13 6:49a007861c76 39
ovidiup13 6:49a007861c76 40 void writeByte(char address, char reg,char value)
ovidiup13 6:49a007861c76 41 // Sends 1 byte to an I2C address
ovidiup13 6:49a007861c76 42 {
ovidiup13 6:49a007861c76 43 i2c.start();
ovidiup13 6:49a007861c76 44 i2c.write(address); // Slave address
ovidiup13 6:49a007861c76 45 i2c.write(reg); // Subaddress (register)
ovidiup13 6:49a007861c76 46 i2c.write(value);
ovidiup13 6:49a007861c76 47 i2c.stop();
ovidiup13 6:49a007861c76 48
ovidiup13 6:49a007861c76 49 }
ovidiup13 6:49a007861c76 50
ovidiup13 6:49a007861c76 51 void initSensors (void)
ovidiup13 6:49a007861c76 52 // Switch on and set up the 3 on-board sensors
ovidiup13 6:49a007861c76 53 {
ovidiup13 6:49a007861c76 54 //pc.printf("--------------------------------------\n");
ovidiup13 6:49a007861c76 55 //pc.printf("\nSTM MEMS eval board sensor init \n");
ovidiup13 6:49a007861c76 56
ovidiup13 6:49a007861c76 57 #ifdef LSM303_on
ovidiup13 6:49a007861c76 58 // LSM303DLHC Magnetic sensor
ovidiup13 6:49a007861c76 59 //pc.printf("LSM303DLHC ping (should reply 0x48): %x \n",readByte(LSM303_m,mIRA_REG_M));
ovidiup13 6:49a007861c76 60 writeByte(LSM303_m,mCRA_REG_M,0x94); //switch on temperature sensor and set Output Data Rate to 30Hz
ovidiup13 6:49a007861c76 61 writeByte(LSM303_m,mCRB_REG_M,0x20); // Set the gain for +/- 1.3 Gauss full scale range
ovidiup13 6:49a007861c76 62 writeByte(LSM303_m,mMR_REG_M,0x0); // Continuous convertion mode
ovidiup13 6:49a007861c76 63
ovidiup13 6:49a007861c76 64 // LSM303DLHC Accelerometer
ovidiup13 6:49a007861c76 65 writeByte(LSM303_a,aCTRL_REG1_A ,0x37); // Set 25Hz ODR, everything else on
ovidiup13 6:49a007861c76 66 writeByte(LSM303_a,aCTRL_REG4_A ,0x08); // Set full scale to +/- 2g sensitivity and high rez mode
ovidiup13 6:49a007861c76 67 #endif
ovidiup13 6:49a007861c76 68
ovidiup13 6:49a007861c76 69 //pc.printf("--------------------------------------\n \n");
ovidiup13 6:49a007861c76 70 wait(2); // Wait for settings to stabilize
ovidiup13 6:49a007861c76 71 }
ovidiup13 6:49a007861c76 72
ovidiup13 6:49a007861c76 73 void LSM303 (SensorState_t * state)
ovidiup13 6:49a007861c76 74 // Magnetometer and accelerometer
ovidiup13 6:49a007861c76 75 {
ovidiup13 6:49a007861c76 76 char xL, xH, yL, yH, zL, zH;
ovidiup13 6:49a007861c76 77 int16_t mX, mY, mZ,aX,aY,aZ;
ovidiup13 6:49a007861c76 78 float pitch,roll,faX,faY;
ovidiup13 6:49a007861c76 79
ovidiup13 6:49a007861c76 80 xL=readByte(LSM303_m,mOUT_X_L_M);
ovidiup13 6:49a007861c76 81 xH=readByte(LSM303_m,mOUT_X_H_M);
ovidiup13 6:49a007861c76 82 yL=readByte(LSM303_m,mOUT_Y_L_M);
ovidiup13 6:49a007861c76 83 yH=readByte(LSM303_m,mOUT_Y_H_M);
ovidiup13 6:49a007861c76 84 zL=readByte(LSM303_m,mOUT_Z_L_M);
ovidiup13 6:49a007861c76 85 zH=readByte(LSM303_m,mOUT_Z_H_M);
ovidiup13 6:49a007861c76 86
ovidiup13 6:49a007861c76 87 mX=(xH<<8) | (xL); // 16-bit 2's complement data
ovidiup13 6:49a007861c76 88 mY=(yH<<8) | (yL);
ovidiup13 6:49a007861c76 89 mZ=(zH<<8) | (zL);
ovidiup13 6:49a007861c76 90
ovidiup13 6:49a007861c76 91 //pc.printf("mX=%hd %X mY=%hd %X mZ=%hd %X \n",mX,mX,mY,mY,mZ,mZ);
ovidiup13 6:49a007861c76 92
ovidiup13 6:49a007861c76 93 mX=mX-Offset_mX; // These are callibration co-efficients to deal with non-zero soft iron magnetic offset
ovidiup13 6:49a007861c76 94 mY=mY-Offset_mY;
ovidiup13 6:49a007861c76 95
ovidiup13 6:49a007861c76 96 xL=readByte(LSM303_a,aOUT_X_L_A);
ovidiup13 6:49a007861c76 97 xH=readByte(LSM303_a,aOUT_X_H_A);
ovidiup13 6:49a007861c76 98 yL=readByte(LSM303_a,aOUT_Y_L_A);
ovidiup13 6:49a007861c76 99 yH=readByte(LSM303_a,aOUT_Y_H_A);
ovidiup13 6:49a007861c76 100 zL=readByte(LSM303_a,aOUT_Z_L_A);
ovidiup13 6:49a007861c76 101 zH=readByte(LSM303_a,aOUT_Z_H_A);
ovidiup13 6:49a007861c76 102
ovidiup13 6:49a007861c76 103 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 104 aY=(signed short) ( (yH<<8) | (yL) ) >> 4;
ovidiup13 6:49a007861c76 105 aZ=(signed short) ( (zH<<8) | (zL) ) >> 4;
ovidiup13 6:49a007861c76 106
ovidiup13 6:49a007861c76 107 //pc.printf("aX=%hd %X aY=%hd %X aZ=%hd %X \n",aX,aX,aY,aY,aZ,aZ);
ovidiup13 6:49a007861c76 108
ovidiup13 6:49a007861c76 109 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 110 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 111 //faZ=((float) aZ) /2000.0; // Not used in a calc so comment out to avoid the compiler warning
ovidiup13 6:49a007861c76 112
ovidiup13 6:49a007861c76 113 // Trigonometry derived from STM app note AN3192 and from WikiRobots
ovidiup13 6:49a007861c76 114 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 115 roll = asin(faY*2/cos(pitch));
ovidiup13 6:49a007861c76 116
ovidiup13 6:49a007861c76 117 float xh = mX * cos(pitch) + mZ * sin(pitch);
ovidiup13 6:49a007861c76 118 float yh = mX * sin(roll) * sin(pitch) + mY * cos(roll) - mZ * sin(roll) * cos(pitch);
ovidiup13 6:49a007861c76 119 float zh = -mX * cos(roll) * sin(pitch) + mY * sin(roll) + mZ * cos(roll) * cos(pitch);
ovidiup13 6:49a007861c76 120
ovidiup13 6:49a007861c76 121 float heading = atan2(yh, xh) * RadtoDeg; // Note use of atan2 rather than atan since better for working with quadrants
ovidiup13 6:49a007861c76 122 if (yh < 0)
ovidiup13 6:49a007861c76 123 heading=360+heading;
ovidiup13 6:49a007861c76 124
ovidiup13 6:49a007861c76 125 state->heading=heading;
ovidiup13 6:49a007861c76 126 state->pitch=pitch;
ovidiup13 6:49a007861c76 127 state->roll=roll;
ovidiup13 6:49a007861c76 128 state->aX = (float)aX;
ovidiup13 6:49a007861c76 129 state->aY = (float)aY;
ovidiup13 6:49a007861c76 130 state->aZ = (float)aZ;
ovidiup13 6:49a007861c76 131 //5.1f
ovidiup13 6:49a007861c76 132 //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 133 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",aX,aY,aZ);
ovidiup13 6:49a007861c76 134
ovidiup13 6:49a007861c76 135 }
ovidiup13 6:49a007861c76 136
ovidiup13 6:49a007861c76 137 //calculates the difference for acceleration in int16_t value
ovidiup13 6:49a007861c76 138 void calc_avrg_ac(Result_avrg* result,int samples){
ovidiup13 6:49a007861c76 139 int i = 0;
ovidiup13 6:49a007861c76 140 result -> aX = 0;
ovidiup13 6:49a007861c76 141 result -> aY = 0;
ovidiup13 6:49a007861c76 142 result -> aZ = 0;
ovidiup13 6:49a007861c76 143 SensorState_t state;
ovidiup13 6:49a007861c76 144 for(i = 0;i<samples;i++){
ovidiup13 6:49a007861c76 145 #ifdef LSM303_on
ovidiup13 6:49a007861c76 146 LSM303(&state);
ovidiup13 6:49a007861c76 147 #endif
ovidiup13 6:49a007861c76 148 result -> aX += state.aX;
ovidiup13 6:49a007861c76 149 result -> aY += state.aY;
ovidiup13 6:49a007861c76 150 result -> aZ += state.aZ;
ovidiup13 6:49a007861c76 151 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",state.aX,state.aY,state.aZ);
ovidiup13 6:49a007861c76 152
ovidiup13 6:49a007861c76 153 wait(0.01);
ovidiup13 6:49a007861c76 154 }
ovidiup13 6:49a007861c76 155 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result->aX,result->aY,result->aZ);
ovidiup13 6:49a007861c76 156 result -> aX = result -> aX / samples;
ovidiup13 6:49a007861c76 157 result -> aY = result -> aY / samples;
ovidiup13 6:49a007861c76 158 result -> aZ = result -> aZ / samples;
ovidiup13 6:49a007861c76 159 //pc.printf("rAcceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result->aX,result->aY,result->aZ);
ovidiup13 6:49a007861c76 160 }
ovidiup13 6:49a007861c76 161
ovidiup13 6:49a007861c76 162 //calculates the difference for orientation in float value
ovidiup13 6:49a007861c76 163 void calc_avrg_or(Result_avrg* result,int samples){
ovidiup13 6:49a007861c76 164 int i = 0;
ovidiup13 6:49a007861c76 165 result -> x = 0;
ovidiup13 6:49a007861c76 166 result -> y = 0;
ovidiup13 6:49a007861c76 167 result -> z = 0;
ovidiup13 6:49a007861c76 168 SensorState_t state;
ovidiup13 6:49a007861c76 169 for(i = 0;i<samples;i++){
ovidiup13 6:49a007861c76 170 #ifdef LSM303_on
ovidiup13 6:49a007861c76 171 LSM303(&state);
ovidiup13 6:49a007861c76 172 #endif
ovidiup13 6:49a007861c76 173 result -> x += state.roll*RadtoDeg;
ovidiup13 6:49a007861c76 174 result -> y += state.pitch*RadtoDeg;
ovidiup13 6:49a007861c76 175 result -> z += state.heading;
ovidiup13 6:49a007861c76 176 //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 177 wait(0.01);
ovidiup13 6:49a007861c76 178 }
ovidiup13 6:49a007861c76 179 //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 180 result -> x = result -> x / samples;
ovidiup13 6:49a007861c76 181 result -> y = result -> y / samples;
ovidiup13 6:49a007861c76 182 result -> z = result -> z / samples;
ovidiup13 6:49a007861c76 183 //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 184 }
ovidiup13 6:49a007861c76 185
ovidiup13 6:49a007861c76 186 //gets the two results and saves the answer in r1 structure
ovidiup13 6:49a007861c76 187 void calc_diff(Result_avrg* r1, Result_avrg* r2){
ovidiup13 6:49a007861c76 188 r1 -> x = abs(r1->x - r2->x);
ovidiup13 6:49a007861c76 189 r1 -> y = abs(r1->y - r2->y);
ovidiup13 6:49a007861c76 190 r1 -> z = abs(r1->z - r2->z);
ovidiup13 6:49a007861c76 191
ovidiup13 6:49a007861c76 192 r1 -> aX = abs(r1->aX - r2->aX);
ovidiup13 6:49a007861c76 193 r1 -> aY = abs(r1->aY - r2->aY);
ovidiup13 6:49a007861c76 194 r1 -> aZ = abs(r1->aZ - r2->aZ);
ovidiup13 6:49a007861c76 195 }
ovidiup13 6:49a007861c76 196
ovidiup13 6:49a007861c76 197 /*
ovidiup13 6:49a007861c76 198
ovidiup13 6:49a007861c76 199 int main()
ovidiup13 6:49a007861c76 200 {
ovidiup13 6:49a007861c76 201 SensorState_t state;
ovidiup13 6:49a007861c76 202 Result_avrg result1;
ovidiup13 6:49a007861c76 203 //Result_avrg result2;
ovidiup13 6:49a007861c76 204 initSensors();
ovidiup13 6:49a007861c76 205 //pc.baud(115200);
ovidiup13 6:49a007861c76 206 calc_avrg_or(&result1,3);
ovidiup13 6:49a007861c76 207 //calc_avrg_ac(&result1,3);
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 //calc_avrg_or(&result2,3);
ovidiup13 6:49a007861c76 211 //calc_avrg_ac(&result2,3);
ovidiup13 6:49a007861c76 212 //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 213 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result2.aX,result2.aY,result2.aZ);
ovidiup13 6:49a007861c76 214 //calc_diff(&result1,&result2);
ovidiup13 6:49a007861c76 215 //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 216 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result1.aX,result1.aY,result1.aZ);
ovidiup13 6:49a007861c76 217 #ifdef LSM303_on
ovidiup13 6:49a007861c76 218 //LSM303(&state);
ovidiup13 6:49a007861c76 219 #endif
ovidiup13 6:49a007861c76 220
ovidiup13 6:49a007861c76 221 //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 222 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",state.aX,state.aY,state.aZ);
ovidiup13 6:49a007861c76 223 //pc.printf("\n");
ovidiup13 6:49a007861c76 224
ovidiup13 6:49a007861c76 225 }
ovidiup13 6:49a007861c76 226 */