Demonstration program for mbed when plugged into the RS EDP (Embedded Development Platform) environment. Requires an EDP-AM-MC1 motor module with DC motor and an SRF08 Ultrasonic Rangefinder.
main.cpp@0:f154cd1e30ff, 2010-12-16 (annotated)
- Committer:
- billmarshall
- Date:
- Thu Dec 16 10:52:45 2010 +0000
- Revision:
- 0:f154cd1e30ff
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
billmarshall | 0:f154cd1e30ff | 1 | // EDP & mbed demonstration program using I2C and PWM: W.G.Marshall 2010 |
billmarshall | 0:f154cd1e30ff | 2 | // Requires EDP-AM-MC1 DC motor module |
billmarshall | 0:f154cd1e30ff | 3 | // Uses SRF08 Ultrasonic Range Finder on I2C address 0xE0 |
billmarshall | 0:f154cd1e30ff | 4 | // Option link J217 on EDP mbed carrier changed from 1-2 to 2-3 |
billmarshall | 0:f154cd1e30ff | 5 | // Jumpers on EDP MC1 module will require setting to suit GPIO signals |
billmarshall | 0:f154cd1e30ff | 6 | |
billmarshall | 0:f154cd1e30ff | 7 | #include "mbed.h" |
billmarshall | 0:f154cd1e30ff | 8 | |
billmarshall | 0:f154cd1e30ff | 9 | I2C sonar(p9, p10); // Define SDA, SCL pins |
billmarshall | 0:f154cd1e30ff | 10 | Serial pc(USBTX, USBRX); // Define Tx, Rx pins |
billmarshall | 0:f154cd1e30ff | 11 | PwmOut motor(p23); // Define PWM output (EDP: EVG0_GPIO40) |
billmarshall | 0:f154cd1e30ff | 12 | DigitalOut Brake(p19); // Define brake (EDP: GPIO0) |
billmarshall | 0:f154cd1e30ff | 13 | DigitalOut Direction(p24); // Define direction (EDP: EVG1_GPIO42) |
billmarshall | 0:f154cd1e30ff | 14 | DigitalOut L1(LED1); // User LEDs form bargraph |
billmarshall | 0:f154cd1e30ff | 15 | DigitalOut L2(LED2); |
billmarshall | 0:f154cd1e30ff | 16 | DigitalOut L3(LED3); |
billmarshall | 0:f154cd1e30ff | 17 | DigitalOut L4(LED4); |
billmarshall | 0:f154cd1e30ff | 18 | |
billmarshall | 0:f154cd1e30ff | 19 | const int addr = 0xE0; // I2C device address for SRF08 |
billmarshall | 0:f154cd1e30ff | 20 | char cmd[2]; |
billmarshall | 0:f154cd1e30ff | 21 | char echo[3]; |
billmarshall | 0:f154cd1e30ff | 22 | // echo[0] = light level |
billmarshall | 0:f154cd1e30ff | 23 | // echo[1] = MSB echo |
billmarshall | 0:f154cd1e30ff | 24 | // echo[2] = LSB echo |
billmarshall | 0:f154cd1e30ff | 25 | float temp8; |
billmarshall | 0:f154cd1e30ff | 26 | float range; |
billmarshall | 0:f154cd1e30ff | 27 | float MotorPWM; |
billmarshall | 0:f154cd1e30ff | 28 | |
billmarshall | 0:f154cd1e30ff | 29 | int main() { |
billmarshall | 0:f154cd1e30ff | 30 | |
billmarshall | 0:f154cd1e30ff | 31 | // Set up SRF08 max range and receiver sensitivity |
billmarshall | 0:f154cd1e30ff | 32 | cmd[0] = 0x02; // Range register |
billmarshall | 0:f154cd1e30ff | 33 | cmd[1] = 0x1C; // Set max range about 100cm |
billmarshall | 0:f154cd1e30ff | 34 | sonar.write(addr, cmd, 2); |
billmarshall | 0:f154cd1e30ff | 35 | cmd[0] = 0x01; // Receiver gain register |
billmarshall | 0:f154cd1e30ff | 36 | cmd[1] = 0x1B; // Set max receiver gain |
billmarshall | 0:f154cd1e30ff | 37 | sonar.write(addr, cmd, 2); |
billmarshall | 0:f154cd1e30ff | 38 | |
billmarshall | 0:f154cd1e30ff | 39 | // Set up PWM and motor control |
billmarshall | 0:f154cd1e30ff | 40 | motor.period(0.00005); // Set PWM frequency = 20kHz |
billmarshall | 0:f154cd1e30ff | 41 | motor.write(1); // Motor speed = 0 |
billmarshall | 0:f154cd1e30ff | 42 | Brake = 0; // Brake on = 1, Brake off = 0 |
billmarshall | 0:f154cd1e30ff | 43 | Direction = 0; |
billmarshall | 0:f154cd1e30ff | 44 | |
billmarshall | 0:f154cd1e30ff | 45 | printf("\n\n\rCollision Avoidance Program Vsn 1.2\n\r"); |
billmarshall | 0:f154cd1e30ff | 46 | printf("\n\r Range Light\n\r"); |
billmarshall | 0:f154cd1e30ff | 47 | |
billmarshall | 0:f154cd1e30ff | 48 | while (1) { |
billmarshall | 0:f154cd1e30ff | 49 | |
billmarshall | 0:f154cd1e30ff | 50 | // Get range data and light level from SRF08 |
billmarshall | 0:f154cd1e30ff | 51 | // Send Tx burst command over I2C bus |
billmarshall | 0:f154cd1e30ff | 52 | cmd[0] = 0x00; // Command register |
billmarshall | 0:f154cd1e30ff | 53 | cmd[1] = 0x51; // Ranging results in cm |
billmarshall | 0:f154cd1e30ff | 54 | sonar.write(addr, cmd, 2); // Send ranging burst |
billmarshall | 0:f154cd1e30ff | 55 | |
billmarshall | 0:f154cd1e30ff | 56 | wait(0.07); |
billmarshall | 0:f154cd1e30ff | 57 | |
billmarshall | 0:f154cd1e30ff | 58 | // Read back light level and range over I2C bus |
billmarshall | 0:f154cd1e30ff | 59 | cmd[0] = 0x01; // Address of light level |
billmarshall | 0:f154cd1e30ff | 60 | sonar.write(addr, cmd, 1, 1); // Send address of light level |
billmarshall | 0:f154cd1e30ff | 61 | sonar.read(addr, echo, 3); // read light and echo result |
billmarshall | 0:f154cd1e30ff | 62 | |
billmarshall | 0:f154cd1e30ff | 63 | // Generate PWM mark/space ratio from range data |
billmarshall | 0:f154cd1e30ff | 64 | range = (echo[1]<<8)+echo[2]; |
billmarshall | 0:f154cd1e30ff | 65 | MotorPWM = range/100; // Turn range into PWM ratio |
billmarshall | 0:f154cd1e30ff | 66 | if (temp8 != 0) { |
billmarshall | 0:f154cd1e30ff | 67 | motor.write(1-MotorPWM); // Update PWM ratio (0 -> 1.0) |
billmarshall | 0:f154cd1e30ff | 68 | } |
billmarshall | 0:f154cd1e30ff | 69 | else { |
billmarshall | 0:f154cd1e30ff | 70 | motor.write(1); // PWM ratio = 0 |
billmarshall | 0:f154cd1e30ff | 71 | } |
billmarshall | 0:f154cd1e30ff | 72 | pc.printf(" %3.0f", range); |
billmarshall | 0:f154cd1e30ff | 73 | pc.printf(" %3d\r", echo[0]); |
billmarshall | 0:f154cd1e30ff | 74 | |
billmarshall | 0:f154cd1e30ff | 75 | // Keyboard input to toggle start/stop motor |
billmarshall | 0:f154cd1e30ff | 76 | if (pc.readable()) { // Check for keyboard input |
billmarshall | 0:f154cd1e30ff | 77 | pc.getc(); // Key pressed, clear buffer |
billmarshall | 0:f154cd1e30ff | 78 | if (temp8 == 0) { // Motor already stopped? |
billmarshall | 0:f154cd1e30ff | 79 | Brake = 0; // Brake off, start motor |
billmarshall | 0:f154cd1e30ff | 80 | temp8 = range; |
billmarshall | 0:f154cd1e30ff | 81 | } |
billmarshall | 0:f154cd1e30ff | 82 | else { // Motor running |
billmarshall | 0:f154cd1e30ff | 83 | Brake = 1; // Brake on, stop motor |
billmarshall | 0:f154cd1e30ff | 84 | temp8 = 0; |
billmarshall | 0:f154cd1e30ff | 85 | } |
billmarshall | 0:f154cd1e30ff | 86 | } |
billmarshall | 0:f154cd1e30ff | 87 | wait(0.1); // Wait for 100ms |
billmarshall | 0:f154cd1e30ff | 88 | |
billmarshall | 0:f154cd1e30ff | 89 | // Bargraph with mbed user LEDs |
billmarshall | 0:f154cd1e30ff | 90 | if (range > 70) { |
billmarshall | 0:f154cd1e30ff | 91 | L1 = 1, L2 = 1, L3 = 1, L4 = 1; |
billmarshall | 0:f154cd1e30ff | 92 | } |
billmarshall | 0:f154cd1e30ff | 93 | else if (range > 40) { |
billmarshall | 0:f154cd1e30ff | 94 | L1 = 1, L2 = 1, L3 = 1, L4 = 0; |
billmarshall | 0:f154cd1e30ff | 95 | } |
billmarshall | 0:f154cd1e30ff | 96 | else if (range > 25) { |
billmarshall | 0:f154cd1e30ff | 97 | L1 = 1, L2 = 1, L3 = 0, L4 = 0; |
billmarshall | 0:f154cd1e30ff | 98 | } |
billmarshall | 0:f154cd1e30ff | 99 | else if (range > 7) { |
billmarshall | 0:f154cd1e30ff | 100 | L1 = 1, L2 = 0, L3 = 0, L4 = 0; |
billmarshall | 0:f154cd1e30ff | 101 | } |
billmarshall | 0:f154cd1e30ff | 102 | else { |
billmarshall | 0:f154cd1e30ff | 103 | L1 = 0, L2 = 0, L3 = 0, L4 = 0; |
billmarshall | 0:f154cd1e30ff | 104 | } |
billmarshall | 0:f154cd1e30ff | 105 | } |
billmarshall | 0:f154cd1e30ff | 106 | } |