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