main.cpp

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