main.cpp

00001 /* Program Example: 13.2 Open Loop Compass 
00002                                                                */
00003 #include "mbed.h"
00004 //***** mbed objects *****
00005 I2C compass(p28, p27);         // sda, scl
00006 PwmOut PWM(p25);
00007 AnalogIn Ain(p20);
00008 Serial pc(USBTX, USBRX);        // tx, rx
00009 Ticker s100hz_tick;             // 100 Hz (10ms) ticker
00010 Ticker s5hz_tick;               // 5 Hz (200ms) ticker
00011 //***** variables *****
00012 const int addr = 0x42;          // define the I2C write Address
00013 char cmd[3];
00014 float pos;                      // measured position
00015 float ctrlval;                  // PWM control value
00016 //***** function prototypes *****
00017 void s100hz_task(void);         // 100 Hz task
00018 void s5hz_task(void);           // 5 Hz task
00019 //***** main code *****
00020 int main() {
00021   // initialise and setup data
00022   PWM.period(0.02);
00023   cmd[0] = 0x47;                 // 'G' write to RAM address
00024   cmd[1] = 0x74;                 // Operation mode register address
00025   cmd[2] = 0x72;                 // Op mode = 20H, S/R, continuous 
00026   compass.write(addr,cmd, 3);    // Send operation
00027   s100hz_tick.attach(&s100hz_task,0.01);     // attach 100 Hz task              
00028   s5hz_tick.attach(&s5hz_task,0.2);          // attach 5 Hz task 
00029   while(1){
00030   // loop forever
00031   }
00032 }
00033 
00034 //***** function 100hz_task *****
00035 void s100hz_task(void) {
00036   compass.read(addr, cmd, 2);     // read the two-byte compass data
00037   pos = 0.1 * ((cmd[0] << 8) + cmd[1]);        //convert to degrees
00038     if (pos>180)
00039       pos=pos-360;                            // convert to ±180deg
00040   ctrlval=Ain;          // set control value (also try ctrlval=Ain/4)
00041   PWM=ctrlval;          // output control value to PWM
00042 }
00043 
00044 //***** function 5hz_task *****
00045 void s5hz_task(void) {
00046   pc.printf("deg = %.1f    PWM = %.4f\n", pos, ctrlval);
00047 }