Diff: main.cpp

Revision:

0:5d512f2b35f8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jun 16 15:38:30 2013 +0000
@@ -0,0 +1,60 @@
+/* Program Example 13.3: Closed loop compass program
+                                                                    */
+#include "mbed.h"
+
+// mbed objects
+I2C compass(p28, p27);        // sda, scl
+PwmOut PWM(p25);
+AnalogIn Ain(p20);
+Serial pc(USBTX, USBRX);         // tx, rx
+Ticker s100hz_tick;              // 100 Hz (10ms) ticker
+Ticker s5hz_tick;                // 5 Hz (200ms) ticker
+
+// variables
+const int addr = 0x42; // define the I2C write Address
+char cmd[3];
+float pos;             // measured position
+float setpos=0;        // setpoint position = zero (North)
+float error;           // calculated error 
+float ctrlval;         // PWM control value
+float kp=0.0002;           // proportional gain
+float PWM_zero=0.075;      // zero value
+
+// function prototypes
+void s100hz_task(void);    // 100 Hz task
+void s5hz_task(void);      // 5 Hz task
+
+// main code
+int main() {
+  // initialise and setup data
+  PWM.period(0.02);
+  cmd[0] = 0x47;                 // 'G' write to RAM address
+  cmd[1] = 0x74;                 // Operation mode register address
+  cmd[2] = 0x72;                 // Op mode = 20H, S/R, continuous 
+  compass.write(addr,cmd, 3);    // Send operation 
+  // assign timers
+  s100hz_tick.attach(&s100hz_task,0.01);  //attach 100 Hz task to 10ms tick
+  s5hz_tick.attach(&s5hz_task,0.2);       //attach 5Hz task to 200ms tick
+  while(1){
+  // loop forever
+  }
+}
+
+// function 100hz_task
+void s100hz_task(void) {                  
+  compass.read(addr, cmd, 2); // read the two-byte echo result 
+  //convert data to degrees
+  pos = 0.1 * ((cmd[0] << 8) + cmd[1]);
+  if (pos>180)
+    pos=pos-360;       
+  error = setpos - pos;           // get error
+  ctrlval = (kp * error);         // calculate ctrlval (proportional)
+  ctrlval = ctrlval + PWM_zero;   // add control value to zero position
+  PWM = ctrlval;                  // output to PWM
+}
+
+// function 5hz_task
+void s5hz_task(void) {
+  pc.printf("deg = %.1f error=%.1f ctrlval=%.4f\n",pos,error,ctrlval);
+}
+