Lab 4CHEL / bertl_libraries

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?

UserRevisionLine numberNew 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 }