main.cpp

00001 #include "mbed.h"
00002 #include "WiGo_BattCharger.h"
00003 #include "TSISensor.h"
00004 #include "math.h"
00005 #include "MPL3115A2.h"
00006 #include "main.h"
00007  
00008 MPL3115A2 wigo_sensor1(PTE0, PTE1, MPL3115A2_I2C_ADDRESS);
00009 
00010 // Some LEDs for showing status
00011 DigitalOut redLed(LED_RED);
00012 DigitalOut greenLed(LED_GREEN);
00013 DigitalOut blueLed(LED_BLUE);
00014 
00015 // test analog input pin
00016 AnalogIn NTC(PTE20);
00017 AnalogIn testPin1(PTE30);
00018 AnalogIn testPin2(PTE22);
00019 AnalogIn testPin3(PTC0);
00020 AnalogIn testPin4(PTE29);
00021 AnalogIn testPin5(PTD1);
00022 
00023 // Slide sensor acts as a button
00024 TSISensor tsi;
00025 
00026 // I2C used to communicate with sensor
00027 I2C i2c(PTE0, PTE1);
00028 
00029 int avgX, avgY, newX, tempXmin, tempXmax, newY, tempYmin, tempYmax;
00030 // Ideally these would be saved in eeprom/flash
00031 struct settings_t {
00032     long maxX, minX, maxY, minY;
00033 }
00034 settings;
00035 
00036 const int addr = MAG_ADDR;
00037 
00038 Serial pc(USBTX, USBRX);
00039 
00040 WiGo_BattCharger Batt( BATT_LOW, BATT_MED, BATT_FULL, CHRG_EN1, CHRG_EN2, CHRG_SNS_EN, CHRG_SNS, CHRG_POK, CHRG_CHG);
00041 
00042 // Read a single byte form 8 bit register, return as int
00043 int readReg(char regAddr)
00044 {
00045     char cmd[1];
00046 
00047     cmd[0] = regAddr;
00048     i2c.write(addr, cmd, 1);
00049 
00050     cmd[0] = 0x00;
00051     i2c.read(addr, cmd, 1);
00052     return (int)( cmd[0]);
00053 }
00054 
00055 // read a register per, pass first reg value, reading 2 bytes increments register
00056 // Reads MSB first then LSB
00057 int readVal(char regAddr)
00058 {
00059     char cmd[2];
00060 
00061     cmd[0] = regAddr;
00062     i2c.write(addr, cmd, 1);
00063 
00064     cmd[0] = 0x00;
00065     cmd[1] = 0x00;
00066     i2c.read(addr, cmd, 2);
00067     return (int)( (cmd[1]|(cmd[0] << 8))); //concatenate the MSB and LSB
00068 }
00069 
00070 void initMag() {
00071     char cmd[2];
00072 
00073     cmd[0] = MAG_CTRL_REG2;
00074     cmd[1] = 0x80;
00075     i2c.write(addr, cmd, 2);
00076 
00077     cmd[0] = MAG_CTRL_REG1;
00078     cmd[1] = MAG_3110_SAMPLE80+MAG_3110_OVERSAMPLE2+MAG_3110_ACTIVE; // 0x91;
00079     i2c.write(addr, cmd, 2);
00080     }
00081     
00082 void calXY() //magnetometer calibration: finding max and min of X, Y axis
00083 {
00084     int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY;
00085     redLed = ON;
00086 
00087     printf("Waiting for initial press\n");
00088     // Wait for slider to be pressed
00089     while( tsi.readDistance() == 0 ) {
00090         redLed = ON;
00091         wait(0.2);
00092         redLed = OFF;
00093         wait(0.2);
00094     }
00095     
00096     printf("Waiting for release\n");
00097 
00098     // Wait for release
00099     while( tsi.readDistance() != 0 ) {
00100         redLed = OFF;
00101         wait(0.2);
00102         redLed = ON;
00103         wait(0.2);
00104     }
00105     redLed = OFF;
00106     wait(0.5);
00107     
00108     printf("Rotate\n");
00109         
00110     tempXmax = tempXmin = readVal(MAG_OUT_X_MSB);
00111     tempYmax = tempYmin = readVal(MAG_OUT_Y_MSB);
00112 
00113     while(tsi.readDistance() == 0) {
00114         greenLed = ON;
00115         wait(0.1);
00116         greenLed = OFF;
00117         wait(0.1);
00118         newX = readVal(MAG_OUT_X_MSB);
00119         newY = readVal(MAG_OUT_Y_MSB);
00120         if (newX > tempXmax) tempXmax = newX;
00121         if (newX < tempXmin) tempXmin = newX;
00122         if (newY > tempYmax) tempYmax = newY;
00123         if (newY < tempYmin) tempYmin = newY;
00124     }
00125 
00126     settings.maxX = tempXmax;
00127     settings.minX = tempXmin;
00128     settings.maxY = tempYmax;
00129     settings.minY = tempYmin;
00130 
00131     //store new X, Y values in EEPROM/Flash
00132 
00133     // Calculate average from min/max
00134     avgX=(settings.maxX+settings.minX)/2;
00135     avgY=(settings.maxY+settings.minY)/2;
00136 
00137     // Wait for release
00138     while( tsi.readDistance() != 0 ) {
00139         greenLed = OFF;
00140         wait(0.2);
00141         greenLed = ON;
00142         wait(0.2);
00143     }
00144     greenLed = OFF;
00145     wait(1.0);
00146 }
00147 
00148 // ================================ MAIN CODE ================================
00149 
00150 float max_batt;
00151 float batt_lvl;
00152 float sensor_data[2];
00153 float testVoltage;
00154 
00155 int main()
00156 {
00157     //unsigned int mode=1;
00158     
00159     testVoltage = testPin.read();
00160     
00161     Batt.init(CHRG_500MA);
00162     Batt.sense_en(1);
00163     wait(0.5);
00164     max_batt = Batt.read();
00165     wigo_sensor1.Oversample_Ratio( OVERSAMPLE_RATIO_32);
00166     wigo_sensor1.Barometric_Mode();
00167 
00168     printf("MAG3110 Test\n");
00169 
00170     redLed = OFF;
00171     greenLed = OFF;
00172     blueLed = OFF;
00173     
00174     initMag();
00175 
00176     // Get some values
00177     printf("DR_STATUS %X\n", readReg( MAG_DR_STATUS ));
00178     printf("WHO_AM_I %X\n", readReg( MAG_WHO_AM_I ));
00179     printf("SYSMOD %X\n", readReg( MAG_SYSMOD ));
00180     printf("DIE_TEMP %d\n", readReg( MAG_DIE_TEMP ));
00181 
00182     printf("OFF_X %d\n", readVal( MAG_OFF_X_MSB ));
00183     printf("OFF_Y %d\n", readVal( MAG_OFF_Y_MSB ));
00184     printf("OFF_Z %d\n", readVal( MAG_OFF_Z_MSB ));
00185 
00186     printf("CTRL_REG1 %X\n", readReg( MAG_CTRL_REG1 ));
00187     printf("CTRL_REG2 %X\n", readReg( MAG_CTRL_REG2 ));
00188 
00189     // Mag calibration or store defaults
00190     //===================================
00191     //printf("calibrate\n");
00192     //calXY();
00193     //printf("....Finished\n");
00194     //printf("avgX = %d, avgY = %d\n", avgX, avgY);
00195     avgX = 64366;
00196     avgY = 1624;
00197 
00198     redLed = OFF;
00199     greenLed = OFF;
00200     blueLed = OFF;
00201 
00202     while(1) {
00203 
00204         wait(2);
00205         int xVal = readVal(MAG_OUT_X_MSB);
00206         int yVal = readVal(MAG_OUT_Y_MSB);
00207         int zVal = readVal(MAG_OUT_Z_MSB);
00208         float heading = (atan2((double)(yVal-avgY),(double)(xVal-avgX)))*180/PI;
00209         
00210         // Do something with heading - display direction and turn on blue LED if heading approx north
00211         if (abs(heading) <= 22.5) { printf("N\n"); blueLed = ON; } else blueLed = OFF;
00212         if (abs(heading) >= 157.5) printf("S\n");
00213         if (heading >= 67.5 && heading <= 112.5) printf("E \n");
00214         if (heading <= -67.5 && heading >= -112.5) printf("W \n");
00215         if (heading > 22.5 && heading < 67.5) printf("NE\n");
00216         if (heading < -22.5 && heading > -67.5) printf("NW\n");
00217         if (heading > 112.5 && heading < 157.5) printf("SE\n");
00218         if (heading < -112.5 && heading > -157.5) printf("SW\n");
00219 
00220         if (heading < 0) heading += 360.0;
00221         //printf("xVal - avgX = %d, yVal - avgY = %d  ", xVal-avgX, yVal-avgY);
00222         //printf("X = %d, Y = %d, Z = %d, Heading %f\n", xVal, yVal, zVal, heading);
00223         printf("Heading %f\n", heading);
00224 
00225         Batt.LEDupdate(1);
00226 
00227         batt_lvl = Batt.read();
00228         if( batt_lvl > max_batt ) {
00229             max_batt = batt_lvl;
00230         }
00231 
00232         pc.printf("Batt = %f Max:%f %i%\n", batt_lvl, max_batt, Batt.level());
00233         
00234         if ( wigo_sensor1.isDataAvailable()) {
00235             wigo_sensor1.getAllData( &sensor_data[0]);
00236             pc.printf("Bar: %f\tTemperature: %f\n", sensor_data[0]/100, sensor_data[1]);
00237         }
00238         
00239         //if ( mode & 0x0001) {
00240         //    pc.printf("\tPressure: %f\tTemperature: %f\r\n", sensor_data[0], sensor_data[1]);
00241         //    wigo_sensor1.Altimeter_Mode();
00242         //} else {
00243         //    pc.printf("\Altitude: %f\tTemperature: %f\r\n", sensor_data[0], sensor_data[1]);
00244         //    wigo_sensor1.Barometric_Mode();
00245         //}
00246         //mode++;
00247         
00248         //testVoltage = testPin.read();
00249         //pc.printf("testVoltage  = %f\n", testVoltage );
00250         //testVoltage = testPin1.read();
00251         //pc.printf("testVoltage1 = %f\n", testVoltage );
00252         //testVoltage = testPin2.read();
00253         //pc.printf("testVoltage2 = %f\n", testVoltage );
00254         //testVoltage = testPin3.read();
00255         //pc.printf("testVoltage3 = %f\n", testVoltage );
00256         //testVoltage = testPin4.read();
00257         //pc.printf("testVoltage4 = %f\n", testVoltage );
00258         pc.printf("------------------------------------------------\n");
00259         
00260     }
00261 }