Collections of BERTL libraries
functions_bertl.cpp@3:1708f20fd55b, 2016-04-18 (annotated)
- Committer:
- DongExpander
- Date:
- Mon Apr 18 12:48:42 2016 +0000
- Revision:
- 3:1708f20fd55b
- Parent:
- 2:4a9ed5ca8a9a
Feature; Added initialize()
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DongExpander | 3:1708f20fd55b | 1 | #include "mbed.h" |
DongExpander | 3:1708f20fd55b | 2 | #include "class_hardware.h" |
DongExpander | 3:1708f20fd55b | 3 | #include "class_software.h" |
DongExpander | 3:1708f20fd55b | 4 | #include "hl_bertl_portex.h" |
DongExpander | 3:1708f20fd55b | 5 | #include "functions_bertl.h" |
DongExpander | 2:4a9ed5ca8a9a | 6 | |
DongExpander | 3:1708f20fd55b | 7 | void line_turn(unsigned char linesegment, int speed, Motor left, Motor right) { |
DongExpander | 3:1708f20fd55b | 8 | |
DongExpander | 3:1708f20fd55b | 9 | if ( speed < 30 ) |
DongExpander | 3:1708f20fd55b | 10 | speed = 30; |
DongExpander | 3:1708f20fd55b | 11 | |
DongExpander | 3:1708f20fd55b | 12 | switch (linesegment) { |
DongExpander | 3:1708f20fd55b | 13 | case 0b00000: left.set(speed * 0.80); right.set(speed * -0.80); break; |
DongExpander | 3:1708f20fd55b | 14 | case 0b00100: left.set(speed * 1.50); right.set(speed * 1.50); break; |
DongExpander | 3:1708f20fd55b | 15 | |
DongExpander | 3:1708f20fd55b | 16 | // Nach links |
DongExpander | 3:1708f20fd55b | 17 | case 0b01100: left.set(speed * 0.65); right.set(speed * 1.00); break; |
DongExpander | 3:1708f20fd55b | 18 | case 0b01000: left.set(speed * 0.35); right.set(speed * 0.80); break; |
DongExpander | 3:1708f20fd55b | 19 | case 0b11000: left.set(speed * 0.30); right.set(speed * 0.90); break; |
DongExpander | 3:1708f20fd55b | 20 | case 0b10000: left.set(speed * -0.40); right.set(speed * 0.40); break; |
DongExpander | 3:1708f20fd55b | 21 | |
DongExpander | 3:1708f20fd55b | 22 | // Nach rechts |
DongExpander | 3:1708f20fd55b | 23 | case 0b00110: left.set(speed * 1.00); right.set(speed * 0.65); break; |
DongExpander | 3:1708f20fd55b | 24 | case 0b00010: left.set(speed * 0.80); right.set(speed * 0.35); break; |
DongExpander | 3:1708f20fd55b | 25 | case 0b00011: left.set(speed * 0.90); right.set(speed * 0.30); break; |
DongExpander | 3:1708f20fd55b | 26 | case 0b00001: left.set(speed * 0.40); right.set(speed * -0.40); break; |
DongExpander | 3:1708f20fd55b | 27 | } |
DongExpander | 3:1708f20fd55b | 28 | } |
DongExpander | 3:1708f20fd55b | 29 | |
DongExpander | 3:1708f20fd55b | 30 | |
DongExpander | 3:1708f20fd55b | 31 | unsigned char sensor_to_byte(IRSensor LL, IRSensor L, IRSensor M, IRSensor R, IRSensor RR) { |
DongExpander | 3:1708f20fd55b | 32 | unsigned char sensor_byte = 0b00000; |
DongExpander | 3:1708f20fd55b | 33 | |
DongExpander | 3:1708f20fd55b | 34 | if (LL.is_black()) sensor_byte += 0b10000; |
DongExpander | 3:1708f20fd55b | 35 | if (L.is_black()) sensor_byte += 0b01000; |
DongExpander | 3:1708f20fd55b | 36 | if (M.is_black()) sensor_byte += 0b00100; |
DongExpander | 3:1708f20fd55b | 37 | if (R.is_black()) sensor_byte += 0b00010; |
DongExpander | 3:1708f20fd55b | 38 | if (RR.is_black()) sensor_byte += 0b00001; |
DongExpander | 3:1708f20fd55b | 39 | |
DongExpander | 3:1708f20fd55b | 40 | return sensor_byte; |
DongExpander | 3:1708f20fd55b | 41 | } |
DongExpander | 3:1708f20fd55b | 42 | |
DongExpander | 3:1708f20fd55b | 43 | void initialize(PinName power_motor, PinName power_ir, PortEx portex) { |
DongExpander | 3:1708f20fd55b | 44 | BusOut power (power_motor, power_ir); |
DongExpander | 3:1708f20fd55b | 45 | power = 0xff; |
DongExpander | 3:1708f20fd55b | 46 | BusOut leds(LED1,LED2,LED3,LED4); |
DongExpander | 3:1708f20fd55b | 47 | leds = 0x00; |
DongExpander | 3:1708f20fd55b | 48 | |
DongExpander | 3:1708f20fd55b | 49 | portex.useISR=0; |
DongExpander | 3:1708f20fd55b | 50 | portex.ClearLeds(); |
DongExpander | 3:1708f20fd55b | 51 | portex.WaitUntilButtonPressed(); |
DongExpander | 3:1708f20fd55b | 52 | } |