Rob Toulson / Mbed 2 deprecated PE_13-03_ClosedLoopCompass

by Rob Toulson and Tim Wilmshurst from textbook "Fast and Effective Embedded Systems Design: Applying the ARM mbed"

Dependencies:   mbed

main.cpp@0:5d512f2b35f8, 2013-06-16 (annotated)

Committer:
robt
Date:
Sun Jun 16 15:38:30 2013 +0000
Revision:
0:5d512f2b35f8
by Rob Toulson and Tim Wilmshurst from textbook "Fast and Effective Embedded Systems Design: Applying the ARM mbed"

Who changed what in which revision?

UserRevisionLine numberNew contents of line
robt 0:5d512f2b35f8 1 /* Program Example 13.3: Closed loop compass program
robt 0:5d512f2b35f8 2 */
robt 0:5d512f2b35f8 3 #include "mbed.h"
robt 0:5d512f2b35f8 4
robt 0:5d512f2b35f8 5 // mbed objects
robt 0:5d512f2b35f8 6 I2C compass(p28, p27); // sda, scl
robt 0:5d512f2b35f8 7 PwmOut PWM(p25);
robt 0:5d512f2b35f8 8 AnalogIn Ain(p20);
robt 0:5d512f2b35f8 9 Serial pc(USBTX, USBRX); // tx, rx
robt 0:5d512f2b35f8 10 Ticker s100hz_tick; // 100 Hz (10ms) ticker
robt 0:5d512f2b35f8 11 Ticker s5hz_tick; // 5 Hz (200ms) ticker
robt 0:5d512f2b35f8 12
robt 0:5d512f2b35f8 13 // variables
robt 0:5d512f2b35f8 14 const int addr = 0x42; // define the I2C write Address
robt 0:5d512f2b35f8 15 char cmd[3];
robt 0:5d512f2b35f8 16 float pos; // measured position
robt 0:5d512f2b35f8 17 float setpos=0; // setpoint position = zero (North)
robt 0:5d512f2b35f8 18 float error; // calculated error
robt 0:5d512f2b35f8 19 float ctrlval; // PWM control value
robt 0:5d512f2b35f8 20 float kp=0.0002; // proportional gain
robt 0:5d512f2b35f8 21 float PWM_zero=0.075; // zero value
robt 0:5d512f2b35f8 22
robt 0:5d512f2b35f8 23 // function prototypes
robt 0:5d512f2b35f8 24 void s100hz_task(void); // 100 Hz task
robt 0:5d512f2b35f8 25 void s5hz_task(void); // 5 Hz task
robt 0:5d512f2b35f8 26
robt 0:5d512f2b35f8 27 // main code
robt 0:5d512f2b35f8 28 int main() {
robt 0:5d512f2b35f8 29 // initialise and setup data
robt 0:5d512f2b35f8 30 PWM.period(0.02);
robt 0:5d512f2b35f8 31 cmd[0] = 0x47; // 'G' write to RAM address
robt 0:5d512f2b35f8 32 cmd[1] = 0x74; // Operation mode register address
robt 0:5d512f2b35f8 33 cmd[2] = 0x72; // Op mode = 20H, S/R, continuous
robt 0:5d512f2b35f8 34 compass.write(addr,cmd, 3); // Send operation
robt 0:5d512f2b35f8 35 // assign timers
robt 0:5d512f2b35f8 36 s100hz_tick.attach(&s100hz_task,0.01); //attach 100 Hz task to 10ms tick
robt 0:5d512f2b35f8 37 s5hz_tick.attach(&s5hz_task,0.2); //attach 5Hz task to 200ms tick
robt 0:5d512f2b35f8 38 while(1){
robt 0:5d512f2b35f8 39 // loop forever
robt 0:5d512f2b35f8 40 }
robt 0:5d512f2b35f8 41 }
robt 0:5d512f2b35f8 42
robt 0:5d512f2b35f8 43 // function 100hz_task
robt 0:5d512f2b35f8 44 void s100hz_task(void) {
robt 0:5d512f2b35f8 45 compass.read(addr, cmd, 2); // read the two-byte echo result
robt 0:5d512f2b35f8 46 //convert data to degrees
robt 0:5d512f2b35f8 47 pos = 0.1 * ((cmd[0] << 8) + cmd[1]);
robt 0:5d512f2b35f8 48 if (pos>180)
robt 0:5d512f2b35f8 49 pos=pos-360;
robt 0:5d512f2b35f8 50 error = setpos - pos; // get error
robt 0:5d512f2b35f8 51 ctrlval = (kp * error); // calculate ctrlval (proportional)
robt 0:5d512f2b35f8 52 ctrlval = ctrlval + PWM_zero; // add control value to zero position
robt 0:5d512f2b35f8 53 PWM = ctrlval; // output to PWM
robt 0:5d512f2b35f8 54 }
robt 0:5d512f2b35f8 55
robt 0:5d512f2b35f8 56 // function 5hz_task
robt 0:5d512f2b35f8 57 void s5hz_task(void) {
robt 0:5d512f2b35f8 58 pc.printf("deg = %.1f error=%.1f ctrlval=%.4f\n",pos,error,ctrlval);
robt 0:5d512f2b35f8 59 }
robt 0:5d512f2b35f8 60