Maxwell Sinclair
/
m3pi_LineFollower_dpn
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
main.cpp@0:e60be1842101, 2010-11-30 (annotated)
- Committer:
- Armless
- Date:
- Tue Nov 30 15:17:45 2010 +0000
- Revision:
- 0:e60be1842101
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| Armless | 0:e60be1842101 | 1 | #include "mbed.h" |
| Armless | 0:e60be1842101 | 2 | #include "m3pi.h" |
| Armless | 0:e60be1842101 | 3 | |
| Armless | 0:e60be1842101 | 4 | BusOut leds(LED1,LED2,LED3,LED4); |
| Armless | 0:e60be1842101 | 5 | m3pi m3pi(p23,p9,p10); |
| Armless | 0:e60be1842101 | 6 | |
| Armless | 0:e60be1842101 | 7 | #define MAX 1.0 |
| Armless | 0:e60be1842101 | 8 | #define MIN 0.2 |
| Armless | 0:e60be1842101 | 9 | |
| Armless | 0:e60be1842101 | 10 | |
| Armless | 0:e60be1842101 | 11 | #define P_TERM 0.8 |
| Armless | 0:e60be1842101 | 12 | #define I_TERM 0 |
| Armless | 0:e60be1842101 | 13 | #define D_TERM 20 |
| Armless | 0:e60be1842101 | 14 | |
| Armless | 0:e60be1842101 | 15 | int main() { |
| Armless | 0:e60be1842101 | 16 | |
| Armless | 0:e60be1842101 | 17 | m3pi.locate(0,1); |
| Armless | 0:e60be1842101 | 18 | for (int countdown=3; countdown >=1; countdown--) { |
| Armless | 0:e60be1842101 | 19 | m3pi.printf("%d", countdown); |
| Armless | 0:e60be1842101 | 20 | wait(1); |
| Armless | 0:e60be1842101 | 21 | m3pi.cls(); |
| Armless | 0:e60be1842101 | 22 | |
| Armless | 0:e60be1842101 | 23 | } |
| Armless | 0:e60be1842101 | 24 | m3pi.cls(); |
| Armless | 0:e60be1842101 | 25 | wait(0.001); |
| Armless | 0:e60be1842101 | 26 | m3pi.printf("GO!"); |
| Armless | 0:e60be1842101 | 27 | wait(1); |
| Armless | 0:e60be1842101 | 28 | m3pi.cls(); |
| Armless | 0:e60be1842101 | 29 | m3pi.printf("VB: %f", m3pi.battery()); |
| Armless | 0:e60be1842101 | 30 | |
| Armless | 0:e60be1842101 | 31 | |
| Armless | 0:e60be1842101 | 32 | float right; |
| Armless | 0:e60be1842101 | 33 | float left; |
| Armless | 0:e60be1842101 | 34 | float position_of_line = 0.0; |
| Armless | 0:e60be1842101 | 35 | float act_position_of_line = 0.0; |
| Armless | 0:e60be1842101 | 36 | float prev_pos_of_line = 0.0; |
| Armless | 0:e60be1842101 | 37 | float derivative,proportional; |
| Armless | 0:e60be1842101 | 38 | float integral = 0; |
| Armless | 0:e60be1842101 | 39 | float power; |
| Armless | 0:e60be1842101 | 40 | m3pi.sensor_auto_calibrate(); |
| Armless | 0:e60be1842101 | 41 | float speed = MAX; |
| Armless | 0:e60be1842101 | 42 | |
| Armless | 0:e60be1842101 | 43 | while (1) { |
| Armless | 0:e60be1842101 | 44 | for (int i=0; i<1000; i++) { |
| Armless | 0:e60be1842101 | 45 | |
| Armless | 0:e60be1842101 | 46 | // Get the position of the line. |
| Armless | 0:e60be1842101 | 47 | act_position_of_line = m3pi.line_position(); |
| Armless | 0:e60be1842101 | 48 | position_of_line = act_position_of_line - 0.40; |
| Armless | 0:e60be1842101 | 49 | |
| Armless | 0:e60be1842101 | 50 | proportional = position_of_line; |
| Armless | 0:e60be1842101 | 51 | // Compute the derivative |
| Armless | 0:e60be1842101 | 52 | derivative = position_of_line - prev_pos_of_line; |
| Armless | 0:e60be1842101 | 53 | // Compute the integral |
| Armless | 0:e60be1842101 | 54 | integral += proportional; |
| Armless | 0:e60be1842101 | 55 | // Remember the last position. |
| Armless | 0:e60be1842101 | 56 | prev_pos_of_line = position_of_line; |
| Armless | 0:e60be1842101 | 57 | // Compute |
| Armless | 0:e60be1842101 | 58 | power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
| Armless | 0:e60be1842101 | 59 | |
| Armless | 0:e60be1842101 | 60 | // Compute new speeds |
| Armless | 0:e60be1842101 | 61 | right = speed+power; |
| Armless | 0:e60be1842101 | 62 | left = speed-power; |
| Armless | 0:e60be1842101 | 63 | |
| Armless | 0:e60be1842101 | 64 | // limit checks |
| Armless | 0:e60be1842101 | 65 | if (right < MIN) |
| Armless | 0:e60be1842101 | 66 | right = MIN; |
| Armless | 0:e60be1842101 | 67 | else if (right > MAX) |
| Armless | 0:e60be1842101 | 68 | right = MAX; |
| Armless | 0:e60be1842101 | 69 | |
| Armless | 0:e60be1842101 | 70 | if (left < MIN) |
| Armless | 0:e60be1842101 | 71 | left = MIN; |
| Armless | 0:e60be1842101 | 72 | else if (left > MAX) |
| Armless | 0:e60be1842101 | 73 | left = MAX; |
| Armless | 0:e60be1842101 | 74 | |
| Armless | 0:e60be1842101 | 75 | if ((position_of_line < 0.20) && (position_of_line > -0.20)) { |
| Armless | 0:e60be1842101 | 76 | m3pi.left_motor(MAX); |
| Armless | 0:e60be1842101 | 77 | m3pi.right_motor(MAX); |
| Armless | 0:e60be1842101 | 78 | |
| Armless | 0:e60be1842101 | 79 | } |
| Armless | 0:e60be1842101 | 80 | |
| Armless | 0:e60be1842101 | 81 | // set speed |
| Armless | 0:e60be1842101 | 82 | m3pi.left_motor(left); |
| Armless | 0:e60be1842101 | 83 | m3pi.right_motor(right); |
| Armless | 0:e60be1842101 | 84 | |
| Armless | 0:e60be1842101 | 85 | //m3pi.cls(); |
| Armless | 0:e60be1842101 | 86 | //m3pi.printf("VB: %f", m3pi.battery()); |
| Armless | 0:e60be1842101 | 87 | } |
| Armless | 0:e60be1842101 | 88 | |
| Armless | 0:e60be1842101 | 89 | |
| Armless | 0:e60be1842101 | 90 | } |
| Armless | 0:e60be1842101 | 91 | } |