Martin Werluschnig / Mbed 2 deprecated MicroMouse

MicroMouse

Dependencies:   BertlLib mbed

main.cpp@0:5206be262f16, 2018-11-15 (annotated)

Committer:
martwerl
Date:
Thu Nov 15 18:05:16 2018 +0000
Revision:
0:5206be262f16
MicroMouse

Who changed what in which revision?

UserRevisionLine numberNew contents of line
martwerl 0:5206be262f16 1
martwerl 0:5206be262f16 2 #include "mbed.h"
martwerl 0:5206be262f16 3 #include "Serial_HL.h"
martwerl 0:5206be262f16 4 #include "Bertl14.h"
martwerl 0:5206be262f16 5 #include "BertlObjects.h"
martwerl 0:5206be262f16 6
martwerl 0:5206be262f16 7 // main=2^0 LS ENC 2^2
martwerl 0:5206be262f16 8 BusOut boardPow(p30, P1_6, P1_7);
martwerl 0:5206be262f16 9
martwerl 0:5206be262f16 10 // ls5 nur beim Betrl15
martwerl 0:5206be262f16 11 // AnalogInHL ls1(p18), ls2(p16), ls3(p19), ls4(p17); // B14
martwerl 0:5206be262f16 12
martwerl 0:5206be262f16 13 AnalogInHL ls1(p18), ls2(p16), ls3(p20), ls4(p19), ls5(p17); // B15
martwerl 0:5206be262f16 14
martwerl 0:5206be262f16 15 //Funktionen
martwerl 0:5206be262f16 16 void ForewardUntilXing();
martwerl 0:5206be262f16 17 void NinetyDegreesLeft();
martwerl 0:5206be262f16 18 void CorrectToRight();
martwerl 0:5206be262f16 19 void CorrectToLeft();
martwerl 0:5206be262f16 20 bool CheckLeftSensor();
martwerl 0:5206be262f16 21 bool CheckLeftFrontSensor();
martwerl 0:5206be262f16 22 bool CheckRightSensor();
martwerl 0:5206be262f16 23 bool CheckRightFrontSensor();
martwerl 0:5206be262f16 24
martwerl 0:5206be262f16 25 //globale Variablen
martwerl 0:5206be262f16 26 int SensorLeft = 0;
martwerl 0:5206be262f16 27 int SensorFrontLeft = 0;
martwerl 0:5206be262f16 28 int SensorRight = 0;
martwerl 0:5206be262f16 29 int SensorFrontRight = 0;
martwerl 0:5206be262f16 30
martwerl 0:5206be262f16 31
martwerl 0:5206be262f16 32 int main(void)
martwerl 0:5206be262f16 33 {
martwerl 0:5206be262f16 34 boardPow=3; wait_ms(10);
martwerl 0:5206be262f16 35 InitBertl();
martwerl 0:5206be262f16 36 pex.useISR=0; leds=9;
martwerl 0:5206be262f16 37 pex.ClearLeds();
martwerl 0:5206be262f16 38
martwerl 0:5206be262f16 39
martwerl 0:5206be262f16 40 while(1)
martwerl 0:5206be262f16 41 {
martwerl 0:5206be262f16 42 ForewardUntilXing();
martwerl 0:5206be262f16 43 NinetyDegreesLeft();
martwerl 0:5206be262f16 44 }
martwerl 0:5206be262f16 45 }
martwerl 0:5206be262f16 46
martwerl 0:5206be262f16 47
martwerl 0:5206be262f16 48 void ForewardUntilXing()
martwerl 0:5206be262f16 49 {
martwerl 0:5206be262f16 50 while (CheckLeftFrontSensor() != 1 && CheckLeftSensor() != 1)
martwerl 0:5206be262f16 51 {
martwerl 0:5206be262f16 52 mL.SetPow(0.3); mR.SetPow(0.3);//geradeaus
martwerl 0:5206be262f16 53 if(/*SensorLeft (zu weit Links)*/)
martwerl 0:5206be262f16 54 {
martwerl 0:5206be262f16 55 CorrectToRight();
martwerl 0:5206be262f16 56 }
martwerl 0:5206be262f16 57 else if(/*SensorRight (zu weit Rechts)*/)
martwerl 0:5206be262f16 58 {
martwerl 0:5206be262f16 59 CorrectToLeft();
martwerl 0:5206be262f16 60 }
martwerl 0:5206be262f16 61 }
martwerl 0:5206be262f16 62 }
martwerl 0:5206be262f16 63
martwerl 0:5206be262f16 64
martwerl 0:5206be262f16 65 void NinetyDegreesLeft()
martwerl 0:5206be262f16 66 {
martwerl 0:5206be262f16 67 for (int i = 0; i <= 10; i++)
martwerl 0:5206be262f16 68 {
martwerl 0:5206be262f16 69 mL.SetPow(0.3); mR.SetPow(0.1);//Linkskurve
martwerl 0:5206be262f16 70 wait_ms(100);
martwerl 0:5206be262f16 71 }
martwerl 0:5206be262f16 72
martwerl 0:5206be262f16 73 }
martwerl 0:5206be262f16 74
martwerl 0:5206be262f16 75
martwerl 0:5206be262f16 76 bool CheckLeftSensor()
martwerl 0:5206be262f16 77 {
martwerl 0:5206be262f16 78 if (SensorLeft > 0)
martwerl 0:5206be262f16 79 {
martwerl 0:5206be262f16 80 return 1;
martwerl 0:5206be262f16 81 }
martwerl 0:5206be262f16 82 else
martwerl 0:5206be262f16 83 {
martwerl 0:5206be262f16 84 return 0;
martwerl 0:5206be262f16 85 }
martwerl 0:5206be262f16 86 }
martwerl 0:5206be262f16 87
martwerl 0:5206be262f16 88
martwerl 0:5206be262f16 89
martwerl 0:5206be262f16 90 bool CheckLeftFrontSensor()
martwerl 0:5206be262f16 91 {
martwerl 0:5206be262f16 92 if (SensorFrontLeft > 0)
martwerl 0:5206be262f16 93 {
martwerl 0:5206be262f16 94 return 1;
martwerl 0:5206be262f16 95 }
martwerl 0:5206be262f16 96 else
martwerl 0:5206be262f16 97 {
martwerl 0:5206be262f16 98 return 0;
martwerl 0:5206be262f16 99 }
martwerl 0:5206be262f16 100 }
martwerl 0:5206be262f16 101
martwerl 0:5206be262f16 102
martwerl 0:5206be262f16 103 bool CheckRightSensor()
martwerl 0:5206be262f16 104 {
martwerl 0:5206be262f16 105 if (SensorRight > 0)
martwerl 0:5206be262f16 106 {
martwerl 0:5206be262f16 107 return 1;
martwerl 0:5206be262f16 108 }
martwerl 0:5206be262f16 109 else
martwerl 0:5206be262f16 110 {
martwerl 0:5206be262f16 111 return 0;
martwerl 0:5206be262f16 112 }
martwerl 0:5206be262f16 113 }
martwerl 0:5206be262f16 114
martwerl 0:5206be262f16 115
martwerl 0:5206be262f16 116 bool CheckRightFrontSensor()
martwerl 0:5206be262f16 117 {
martwerl 0:5206be262f16 118 if (SensorFrontRight > 0)
martwerl 0:5206be262f16 119 {
martwerl 0:5206be262f16 120 return 1;
martwerl 0:5206be262f16 121 }
martwerl 0:5206be262f16 122 else
martwerl 0:5206be262f16 123 {
martwerl 0:5206be262f16 124 return 0;
martwerl 0:5206be262f16 125 }
martwerl 0:5206be262f16 126 }
martwerl 0:5206be262f16 127
martwerl 0:5206be262f16 128
martwerl 0:5206be262f16 129 void CorrectToLeft()
martwerl 0:5206be262f16 130 {
martwerl 0:5206be262f16 131 mL.SetPow(0.3); mR.SetPow(0.1);//nach links
martwerl 0:5206be262f16 132 wait_ms(100);
martwerl 0:5206be262f16 133 mL.SetPow(0.3); mR.SetPow(0.3);//wieder geradeaus
martwerl 0:5206be262f16 134 }
martwerl 0:5206be262f16 135
martwerl 0:5206be262f16 136
martwerl 0:5206be262f16 137 void CorrectToRight()
martwerl 0:5206be262f16 138 {
martwerl 0:5206be262f16 139 mL.SetPow(0.1); mR.SetPow(0.3);//nach rechts
martwerl 0:5206be262f16 140 wait_ms(100);
martwerl 0:5206be262f16 141 mL.SetPow(0.3); mR.SetPow(0.3);//wieder geradeaus
martwerl 0:5206be262f16 142 }