![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
xx
Fork of PES1 by
Roboter.cpp@9:d9e46f9c9e40, 2017-04-19 (annotated)
- Committer:
- itslinear
- Date:
- Wed Apr 19 10:07:55 2017 +0000
- Revision:
- 9:d9e46f9c9e40
- Parent:
- 8:6d9cd5ad332d
- Child:
- 17:caf5ae550f2e
tada
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
itslinear | 0:306a2438de17 | 1 | #include "Roboter.h" |
itslinear | 0:306a2438de17 | 2 | |
itslinear | 0:306a2438de17 | 3 | |
itslinear | 4:fdcf3b5009c6 | 4 | Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* s1, Servo* s2,DigitalIn* switch1, DigitalIn* switch2) |
itslinear | 4:fdcf3b5009c6 | 5 | |
itslinear | 1:4e84271a70c6 | 6 | { |
itslinear | 0:306a2438de17 | 7 | |
itslinear | 1:4e84271a70c6 | 8 | sensors[0].init( distance, bit0, bit1, bit2, 0); |
itslinear | 1:4e84271a70c6 | 9 | sensors[1].init( distance, bit0, bit1, bit2, 1); |
itslinear | 1:4e84271a70c6 | 10 | sensors[2].init( distance, bit0, bit1, bit2, 2); |
itslinear | 1:4e84271a70c6 | 11 | sensors[3].init( distance, bit0, bit1, bit2, 3); |
itslinear | 1:4e84271a70c6 | 12 | sensors[4].init( distance, bit0, bit1, bit2, 4); |
itslinear | 1:4e84271a70c6 | 13 | sensors[5].init( distance, bit0, bit1, bit2, 5); |
itslinear | 0:306a2438de17 | 14 | |
itslinear | 1:4e84271a70c6 | 15 | this->pwmR = pwmR; |
itslinear | 1:4e84271a70c6 | 16 | this->pwmL = pwmL; |
itslinear | 4:fdcf3b5009c6 | 17 | this->s1 = s1; |
itslinear | 4:fdcf3b5009c6 | 18 | this->s2 = s2; |
itslinear | 4:fdcf3b5009c6 | 19 | this->switch1 = switch1; |
itslinear | 4:fdcf3b5009c6 | 20 | this->switch2 = switch2; |
itslinear | 4:fdcf3b5009c6 | 21 | |
itslinear | 1:4e84271a70c6 | 22 | |
itslinear | 1:4e84271a70c6 | 23 | } |
itslinear | 1:4e84271a70c6 | 24 | |
itslinear | 1:4e84271a70c6 | 25 | float Roboter::readSensor1() |
itslinear | 0:306a2438de17 | 26 | { |
itslinear | 1:4e84271a70c6 | 27 | return sensors[0].read(); |
itslinear | 1:4e84271a70c6 | 28 | |
itslinear | 0:306a2438de17 | 29 | } |
itslinear | 0:306a2438de17 | 30 | |
itslinear | 1:4e84271a70c6 | 31 | |
itslinear | 1:4e84271a70c6 | 32 | void Roboter::bandeAusweichen() |
itslinear | 0:306a2438de17 | 33 | { |
itslinear | 1:4e84271a70c6 | 34 | float offsetDir; |
itslinear | 1:4e84271a70c6 | 35 | float offsetLin; |
itslinear | 1:4e84271a70c6 | 36 | |
itslinear | 1:4e84271a70c6 | 37 | float x=0.13f; // Distanz ab welcher sensoren reagieren sollen |
itslinear | 1:4e84271a70c6 | 38 | |
itslinear | 6:4af735d26b7a | 39 | offsetDir = 0; |
itslinear | 6:4af735d26b7a | 40 | offsetLin = 0; |
itslinear | 1:4e84271a70c6 | 41 | |
itslinear | 9:d9e46f9c9e40 | 42 | while(sensors[0] < x && sensors[1] < x && sensors[5] < x) { // alle sensoren aktiv, roboter fährt nach hinten |
itslinear | 9:d9e46f9c9e40 | 43 | offsetLin = 0.1f; |
itslinear | 9:d9e46f9c9e40 | 44 | *pwmL = 0.5f+offsetDir-offsetLin; // Setzt die Duty-Cycle auf 50% |
itslinear | 9:d9e46f9c9e40 | 45 | *pwmR = 0.5f+offsetDir+offsetLin; |
itslinear | 9:d9e46f9c9e40 | 46 | |
itslinear | 9:d9e46f9c9e40 | 47 | } |
itslinear | 9:d9e46f9c9e40 | 48 | |
itslinear | 9:d9e46f9c9e40 | 49 | while(sensors[0].read() < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links |
itslinear | 9:d9e46f9c9e40 | 50 | offsetDir = -0.05; |
itslinear | 9:d9e46f9c9e40 | 51 | *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50% |
itslinear | 9:d9e46f9c9e40 | 52 | *pwmR = 0.5f+offsetDir-offsetLin; |
itslinear | 9:d9e46f9c9e40 | 53 | |
itslinear | 6:4af735d26b7a | 54 | } |
itslinear | 0:306a2438de17 | 55 | |
itslinear | 9:d9e46f9c9e40 | 56 | while(sensors[1] < x) { // sensor rechts, roboter dreht nach links |
itslinear | 1:4e84271a70c6 | 57 | offsetDir = -0.05; |
itslinear | 9:d9e46f9c9e40 | 58 | *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50% |
itslinear | 9:d9e46f9c9e40 | 59 | *pwmR = 0.5f+offsetDir-offsetLin; |
itslinear | 9:d9e46f9c9e40 | 60 | |
itslinear | 1:4e84271a70c6 | 61 | } |
itslinear | 1:4e84271a70c6 | 62 | |
itslinear | 9:d9e46f9c9e40 | 63 | while(sensors[5] < x && sensors[1]>(x+0.02f)) { // sensor links, roboter dreht nach rechts |
itslinear | 9:d9e46f9c9e40 | 64 | offsetDir = 0.08; |
itslinear | 9:d9e46f9c9e40 | 65 | *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50% |
itslinear | 9:d9e46f9c9e40 | 66 | *pwmR = 0.5f+offsetDir-offsetLin; |
itslinear | 9:d9e46f9c9e40 | 67 | |
itslinear | 9:d9e46f9c9e40 | 68 | } |
itslinear | 1:4e84271a70c6 | 69 | |
itslinear | 3:24e098715b78 | 70 | } |
schneju2 | 2:953263712480 | 71 | |
schneju2 | 2:953263712480 | 72 | |
schneju2 | 2:953263712480 | 73 | |
itslinear | 1:4e84271a70c6 | 74 | |
itslinear | 3:24e098715b78 | 75 | void Roboter::pickup () |
itslinear | 3:24e098715b78 | 76 | { |
itslinear | 3:24e098715b78 | 77 | |
itslinear | 0:306a2438de17 | 78 | |
schneju2 | 2:953263712480 | 79 | |
itslinear | 3:24e098715b78 | 80 | } |