by Rob Toulson and Tim Wilmshurst from textbook "Fast and Effective Embedded Systems Design: Applying the ARM 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?
User | Revision | Line number | New 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 |