ABELI / Mbed 2 deprecated bertlwallandreturn

KreuHatzl

Dependencies:   BertlLib mbed

main.cpp@0:3aa1e3850dcc, 2015-10-21 (annotated)

Committer:
MarkusSchieder
Date:
Wed Oct 21 17:39:45 2015 +0000
Revision:
0:3aa1e3850dcc
KreuHatzl

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MarkusSchieder 0:3aa1e3850dcc 1
MarkusSchieder 0:3aa1e3850dcc 2 #include "mbed.h"
MarkusSchieder 0:3aa1e3850dcc 3 #include "Serial_HL.h"
MarkusSchieder 0:3aa1e3850dcc 4 #include "Bertl14.h"
MarkusSchieder 0:3aa1e3850dcc 5 #include "BertlObjects.h"
MarkusSchieder 0:3aa1e3850dcc 6
MarkusSchieder 0:3aa1e3850dcc 7 void DriveUntilObstacle();
MarkusSchieder 0:3aa1e3850dcc 8 void Turn180();
MarkusSchieder 0:3aa1e3850dcc 9 void BlinkTask();
MarkusSchieder 0:3aa1e3850dcc 10 Timer t1;
MarkusSchieder 0:3aa1e3850dcc 11 int i,j;
MarkusSchieder 0:3aa1e3850dcc 12
MarkusSchieder 0:3aa1e3850dcc 13 int main(void)
MarkusSchieder 0:3aa1e3850dcc 14 {
MarkusSchieder 0:3aa1e3850dcc 15 i=0;
MarkusSchieder 0:3aa1e3850dcc 16 j=0;
MarkusSchieder 0:3aa1e3850dcc 17 t1.start();
MarkusSchieder 0:3aa1e3850dcc 18 InitBertl();
MarkusSchieder 0:3aa1e3850dcc 19 pex.ClearLeds();
MarkusSchieder 0:3aa1e3850dcc 20 leds = 9;
MarkusSchieder 0:3aa1e3850dcc 21 pex.WaitUntilFrontButtonPressed();
MarkusSchieder 0:3aa1e3850dcc 22 leds=6;
MarkusSchieder 0:3aa1e3850dcc 23 wait(0.8);
MarkusSchieder 0:3aa1e3850dcc 24 pex.WaitUntilFrontButtonPressed();
MarkusSchieder 0:3aa1e3850dcc 25 leds=0;
MarkusSchieder 0:3aa1e3850dcc 26
MarkusSchieder 0:3aa1e3850dcc 27 while(1) {
MarkusSchieder 0:3aa1e3850dcc 28 DriveUntilObstacle();
MarkusSchieder 0:3aa1e3850dcc 29 wait(1);
MarkusSchieder 0:3aa1e3850dcc 30 Turn180();
MarkusSchieder 0:3aa1e3850dcc 31 wait(1);
MarkusSchieder 0:3aa1e3850dcc 32 }
MarkusSchieder 0:3aa1e3850dcc 33 return 1;
MarkusSchieder 0:3aa1e3850dcc 34 }
MarkusSchieder 0:3aa1e3850dcc 35
MarkusSchieder 0:3aa1e3850dcc 36 void DriveUntilObstacle()
MarkusSchieder 0:3aa1e3850dcc 37 {
MarkusSchieder 0:3aa1e3850dcc 38 // Motoren ein
MarkusSchieder 0:3aa1e3850dcc 39 mL.SetPow(0.3);
MarkusSchieder 0:3aa1e3850dcc 40 mR.SetPow(0.3);
MarkusSchieder 0:3aa1e3850dcc 41 // in einer While-Schleife warten bis Distanz kleiner MinDistance
MarkusSchieder 0:3aa1e3850dcc 42 while(1) {
MarkusSchieder 0:3aa1e3850dcc 43 //mL.SetPow(0.5);
MarkusSchieder 0:3aa1e3850dcc 44 //mR.SetPow(0.5);
MarkusSchieder 0:3aa1e3850dcc 45 us.StartMeas();
MarkusSchieder 0:3aa1e3850dcc 46 wait_ms(10);
MarkusSchieder 0:3aa1e3850dcc 47 BlinkTask();
MarkusSchieder 0:3aa1e3850dcc 48 if ((us.distCM*100.0) < 500.0) {
MarkusSchieder 0:3aa1e3850dcc 49 // Motoren aus
MarkusSchieder 0:3aa1e3850dcc 50 if (i==0){
MarkusSchieder 0:3aa1e3850dcc 51 i=1;
MarkusSchieder 0:3aa1e3850dcc 52 }
MarkusSchieder 0:3aa1e3850dcc 53 else {
MarkusSchieder 0:3aa1e3850dcc 54 i=0;
MarkusSchieder 0:3aa1e3850dcc 55 }
MarkusSchieder 0:3aa1e3850dcc 56 mL.SetPow(0.0);
MarkusSchieder 0:3aa1e3850dcc 57 mR.SetPow(0.0);
MarkusSchieder 0:3aa1e3850dcc 58 pex.ClearLeds();
MarkusSchieder 0:3aa1e3850dcc 59 break;
MarkusSchieder 0:3aa1e3850dcc 60 }
MarkusSchieder 0:3aa1e3850dcc 61 }
MarkusSchieder 0:3aa1e3850dcc 62 }
MarkusSchieder 0:3aa1e3850dcc 63
MarkusSchieder 0:3aa1e3850dcc 64 void Turn180()
MarkusSchieder 0:3aa1e3850dcc 65 {
MarkusSchieder 0:3aa1e3850dcc 66 if (j==0){
MarkusSchieder 0:3aa1e3850dcc 67 // Motoren auf 0.5 bis 0.7
MarkusSchieder 0:3aa1e3850dcc 68 mL.SetPow(0.7);
MarkusSchieder 0:3aa1e3850dcc 69 mR.SetPow(-0.7);
MarkusSchieder 0:3aa1e3850dcc 70 j=1;
MarkusSchieder 0:3aa1e3850dcc 71 pex.SetLeds(LED_FR1|LED_BR1);
MarkusSchieder 0:3aa1e3850dcc 72 }
MarkusSchieder 0:3aa1e3850dcc 73 else{
MarkusSchieder 0:3aa1e3850dcc 74 mL.SetPow(-0.7);
MarkusSchieder 0:3aa1e3850dcc 75 mR.SetPow(0.7);
MarkusSchieder 0:3aa1e3850dcc 76 j=0;
MarkusSchieder 0:3aa1e3850dcc 77 pex.SetLeds(LED_FL1|LED_BL1);
MarkusSchieder 0:3aa1e3850dcc 78 }
MarkusSchieder 0:3aa1e3850dcc 79 mL.encCnt=mR.encCnt=0;
MarkusSchieder 0:3aa1e3850dcc 80
MarkusSchieder 0:3aa1e3850dcc 81 // in einer While-Schleife warten bis
MarkusSchieder 0:3aa1e3850dcc 82 // bei !!!beiden!!! Motoren die Ticks für eine 180° Drehung erreicht sind
MarkusSchieder 0:3aa1e3850dcc 83 while (1) {
MarkusSchieder 0:3aa1e3850dcc 84 if (mL.encCnt>20) {
MarkusSchieder 0:3aa1e3850dcc 85 mL.SetPow(0.0);
MarkusSchieder 0:3aa1e3850dcc 86 }
MarkusSchieder 0:3aa1e3850dcc 87 if (mR.encCnt>20) {
MarkusSchieder 0:3aa1e3850dcc 88 mR.SetPow(0.0);
MarkusSchieder 0:3aa1e3850dcc 89 }
MarkusSchieder 0:3aa1e3850dcc 90 if (mL.encCnt>20 && mR.encCnt>20)
MarkusSchieder 0:3aa1e3850dcc 91 pex.ClearLeds();
MarkusSchieder 0:3aa1e3850dcc 92 break;
MarkusSchieder 0:3aa1e3850dcc 93 }
MarkusSchieder 0:3aa1e3850dcc 94 // Motoren ausschalten
MarkusSchieder 0:3aa1e3850dcc 95 mL.SetPow(0.0);
MarkusSchieder 0:3aa1e3850dcc 96 mR.SetPow(0.0);
MarkusSchieder 0:3aa1e3850dcc 97
MarkusSchieder 0:3aa1e3850dcc 98 }
MarkusSchieder 0:3aa1e3850dcc 99
MarkusSchieder 0:3aa1e3850dcc 100 void BlinkTask()
MarkusSchieder 0:3aa1e3850dcc 101 {
MarkusSchieder 0:3aa1e3850dcc 102
MarkusSchieder 0:3aa1e3850dcc 103 if (t1.read_ms()>100) {
MarkusSchieder 0:3aa1e3850dcc 104 if (i==0){
MarkusSchieder 0:3aa1e3850dcc 105 pex.SetLeds(LED_FR1|LED_FL1);
MarkusSchieder 0:3aa1e3850dcc 106 }
MarkusSchieder 0:3aa1e3850dcc 107 else {
MarkusSchieder 0:3aa1e3850dcc 108 pex.SetLeds(LED_BR1|LED_BL1);
MarkusSchieder 0:3aa1e3850dcc 109 }
MarkusSchieder 0:3aa1e3850dcc 110 }
MarkusSchieder 0:3aa1e3850dcc 111 if (t1.read_ms()>200){
MarkusSchieder 0:3aa1e3850dcc 112 pex.ClearLeds();
MarkusSchieder 0:3aa1e3850dcc 113 t1.reset();
MarkusSchieder 0:3aa1e3850dcc 114 }
MarkusSchieder 0:3aa1e3850dcc 115 }
MarkusSchieder 0:3aa1e3850dcc 116