Pathfinding nach rechts funktioniert noch nicht...der rest schon
Dependencies: mbed
Fork of MicroMouse_MASTER_THREE by
Drive.cpp@2:592f01278db4, 2018-04-04 (annotated)
- Committer:
- ruesipat
- Date:
- Wed Apr 04 15:24:28 2018 +0000
- Revision:
- 2:592f01278db4
- Parent:
- 1:d9e840c48b1e
- Child:
- 3:2ec7cf8bc3fc
p
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ruesipat | 1:d9e840c48b1e | 1 | #include <cmath> |
ruesipat | 1:d9e840c48b1e | 2 | #include "Drive.h" |
ruesipat | 1:d9e840c48b1e | 3 | |
ruesipat | 1:d9e840c48b1e | 4 | |
ruesipat | 1:d9e840c48b1e | 5 | |
ruesipat | 1:d9e840c48b1e | 6 | using namespace std; |
ruesipat | 1:d9e840c48b1e | 7 | |
ruesipat | 2:592f01278db4 | 8 | const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit |
ruesipat | 2:592f01278db4 | 9 | const int Drive::DRIVINGCOUNTS = 1712; //Entspricht Strecke von 20cm |
ruesipat | 1:d9e840c48b1e | 10 | |
ruesipat | 2:592f01278db4 | 11 | Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront, float distanceLeftBack): |
ruesipat | 1:d9e840c48b1e | 12 | kontrastSensor(kontrastSensor), |
ruesipat | 1:d9e840c48b1e | 13 | counterLeft(counterLeft), |
ruesipat | 1:d9e840c48b1e | 14 | counterRight(counterRight), |
ruesipat | 1:d9e840c48b1e | 15 | controller(controller) |
ruesipat | 1:d9e840c48b1e | 16 | { |
ruesipat | 1:d9e840c48b1e | 17 | this->distanceRight = distanceRight; |
ruesipat | 1:d9e840c48b1e | 18 | this->distanceFront = distanceFront; |
ruesipat | 1:d9e840c48b1e | 19 | this->distanceLeftFront = distanceLeftFront; |
ruesipat | 2:592f01278db4 | 20 | this->distanceLeftBack = distanceLeftBack; |
ruesipat | 1:d9e840c48b1e | 21 | } |
ruesipat | 1:d9e840c48b1e | 22 | |
ruesipat | 1:d9e840c48b1e | 23 | Drive::~Drive() {} |
ruesipat | 1:d9e840c48b1e | 24 | |
ruesipat | 1:d9e840c48b1e | 25 | |
ruesipat | 1:d9e840c48b1e | 26 | void Drive::driving() |
ruesipat | 1:d9e840c48b1e | 27 | { |
ruesipat | 1:d9e840c48b1e | 28 | |
ruesipat | 1:d9e840c48b1e | 29 | int countsRight = counterRight.read(); //EncoderCounts auslesen |
ruesipat | 1:d9e840c48b1e | 30 | int countsRight0 = countsRight; //ReferenzCounts setzten |
ruesipat | 1:d9e840c48b1e | 31 | int countsLeft = counterLeft.read(); |
ruesipat | 1:d9e840c48b1e | 32 | int countsLeft0 = countsLeft; |
ruesipat | 1:d9e840c48b1e | 33 | |
ruesipat | 1:d9e840c48b1e | 34 | printf("Los gehts\n"); |
ruesipat | 1:d9e840c48b1e | 35 | |
ruesipat | 2:592f01278db4 | 36 | while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS)){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! |
ruesipat | 1:d9e840c48b1e | 37 | |
ruesipat | 1:d9e840c48b1e | 38 | //kontrastSensor.check(); |
ruesipat | 1:d9e840c48b1e | 39 | controller.setDesiredSpeedRight(DRIVINGSPEED); |
ruesipat | 1:d9e840c48b1e | 40 | controller.setDesiredSpeedLeft(-DRIVINGSPEED); |
ruesipat | 1:d9e840c48b1e | 41 | countsRight = counterRight.read(); |
ruesipat | 1:d9e840c48b1e | 42 | countsLeft = counterLeft.read(); |
ruesipat | 2:592f01278db4 | 43 | //printf("CountsRight%d\n\r", countsRight); |
ruesipat | 2:592f01278db4 | 44 | //printf("CountsLeft%d\n\r", countsLeft); |
ruesipat | 1:d9e840c48b1e | 45 | |
ruesipat | 2:592f01278db4 | 46 | |
ruesipat | 2:592f01278db4 | 47 | |
ruesipat | 2:592f01278db4 | 48 | //(distanceLeftFront - distanceRight) /rotationsgeschwindigkeit und allenfalls korrektur!!!!!!!!!!!!!! |
ruesipat | 1:d9e840c48b1e | 49 | |
ruesipat | 1:d9e840c48b1e | 50 | } |
ruesipat | 1:d9e840c48b1e | 51 | controller.setDesiredSpeedRight(0.0f); |
ruesipat | 1:d9e840c48b1e | 52 | controller.setDesiredSpeedLeft(0.0f); |
ruesipat | 1:d9e840c48b1e | 53 | |
ruesipat | 1:d9e840c48b1e | 54 | } |