Helvijs Kiselis
/
MicromousePES2
main.cpp: Sensoren einlesen und Motoren ansteuern
Motion.cpp@4:e74c06e43485, 2018-04-11 (annotated)
- Committer:
- Helvis
- Date:
- Wed Apr 11 15:26:03 2018 +0000
- Revision:
- 4:e74c06e43485
- Parent:
- 1:1adf5dfcc7bb
- Child:
- 5:47262622a9bf
V0.2
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Helvis | 0:9a3e7847a4be | 1 | |
Helvis | 0:9a3e7847a4be | 2 | #include <cmath> |
Helvis | 0:9a3e7847a4be | 3 | #include "Motion.h" |
Helvis | 0:9a3e7847a4be | 4 | |
Helvis | 0:9a3e7847a4be | 5 | using namespace std; |
Helvis | 1:1adf5dfcc7bb | 6 | |
Helvis | 4:e74c06e43485 | 7 | const float Motion::SPEEDLEFT = 50.0f; |
Helvis | 4:e74c06e43485 | 8 | const float Motion::SPEEDRIGHT = 50.0f; |
Helvis | 1:1adf5dfcc7bb | 9 | |
Helvis | 1:1adf5dfcc7bb | 10 | |
Helvis | 1:1adf5dfcc7bb | 11 | Motion::Motion(Controller& controller, EncoderCounter& counterLeft, |
Helvis | 1:1adf5dfcc7bb | 12 | EncoderCounter& counterRight, IRSensor& irSensorL, |
Helvis | 1:1adf5dfcc7bb | 13 | IRSensor& irSensorC, IRSensor& irSensorR) : |
Helvis | 1:1adf5dfcc7bb | 14 | controller(controller), counterLeft(counterLeft), |
Helvis | 1:1adf5dfcc7bb | 15 | counterRight(counterRight), irSensorL(irSensorL), |
Helvis | 1:1adf5dfcc7bb | 16 | irSensorC(irSensorC), irSensorR(irSensorR) { |
Helvis | 1:1adf5dfcc7bb | 17 | |
Helvis | 1:1adf5dfcc7bb | 18 | countsLeft = 0; |
Helvis | 1:1adf5dfcc7bb | 19 | countsRight = 0; |
Helvis | 1:1adf5dfcc7bb | 20 | } |
Helvis | 1:1adf5dfcc7bb | 21 | |
Helvis | 1:1adf5dfcc7bb | 22 | Motion::~Motion() {} |
Helvis | 1:1adf5dfcc7bb | 23 | |
Helvis | 1:1adf5dfcc7bb | 24 | void Motion::move() { |
Helvis | 1:1adf5dfcc7bb | 25 | |
Helvis | 1:1adf5dfcc7bb | 26 | distanceL = irSensorL.read(); |
Helvis | 1:1adf5dfcc7bb | 27 | distanceC = irSensorC.read(); |
Helvis | 1:1adf5dfcc7bb | 28 | distanceR = irSensorR.read(); |
Helvis | 1:1adf5dfcc7bb | 29 | |
Helvis | 4:e74c06e43485 | 30 | while(countsLeft < 200 || countsRight < 200) { |
Helvis | 4:e74c06e43485 | 31 | //Counts für eine Feld Bewegung |
Helvis | 4:e74c06e43485 | 32 | countsLeft = counterLeft.read(); |
Helvis | 4:e74c06e43485 | 33 | countsRight = counterRight.read(); |
Helvis | 4:e74c06e43485 | 34 | |
Helvis | 4:e74c06e43485 | 35 | controller.setDesiredSpeedLeft(50.0f); |
Helvis | 4:e74c06e43485 | 36 | controller.setDesiredSpeedRight(-50.0f); |
Helvis | 4:e74c06e43485 | 37 | |
Helvis | 4:e74c06e43485 | 38 | } |
Helvis | 1:1adf5dfcc7bb | 39 | } |
Helvis | 1:1adf5dfcc7bb | 40 | |
Helvis | 1:1adf5dfcc7bb | 41 | void Motion::rotateL() { |
Helvis | 1:1adf5dfcc7bb | 42 | |
Helvis | 1:1adf5dfcc7bb | 43 | while(countsLeft < 200 || countsRight > -200) { |
Helvis | 1:1adf5dfcc7bb | 44 | //Count Grenze durch Rechnung bestimmen für 90° Umfang/4 |
Helvis | 1:1adf5dfcc7bb | 45 | countsLeft = counterLeft.read(); |
Helvis | 1:1adf5dfcc7bb | 46 | countsRight = counterRight.read(); |
Helvis | 1:1adf5dfcc7bb | 47 | |
Helvis | 1:1adf5dfcc7bb | 48 | controller.setDesiredSpeedLeft(50.0f); |
Helvis | 1:1adf5dfcc7bb | 49 | controller.setDesiredSpeedRight(-50.0f); |
Helvis | 1:1adf5dfcc7bb | 50 | |
Helvis | 1:1adf5dfcc7bb | 51 | } |
Helvis | 1:1adf5dfcc7bb | 52 | } |
Helvis | 1:1adf5dfcc7bb | 53 | |
Helvis | 1:1adf5dfcc7bb | 54 | void Motion::rotateR() { |
Helvis | 1:1adf5dfcc7bb | 55 | |
Helvis | 1:1adf5dfcc7bb | 56 | while(countsLeft > -200 || countsRight < 200) { |
Helvis | 1:1adf5dfcc7bb | 57 | //Count Grenze durch Rechnung bestimmen für 90° Umfang/4 |
Helvis | 1:1adf5dfcc7bb | 58 | countsLeft = counterLeft.read(); |
Helvis | 1:1adf5dfcc7bb | 59 | countsRight = counterRight.read(); |
Helvis | 1:1adf5dfcc7bb | 60 | |
Helvis | 1:1adf5dfcc7bb | 61 | controller.setDesiredSpeedLeft(-50.0f); |
Helvis | 1:1adf5dfcc7bb | 62 | controller.setDesiredSpeedRight(50.0f); |
Helvis | 1:1adf5dfcc7bb | 63 | |
Helvis | 1:1adf5dfcc7bb | 64 | } |
Helvis | 1:1adf5dfcc7bb | 65 | } |
Helvis | 1:1adf5dfcc7bb | 66 | |
Helvis | 1:1adf5dfcc7bb | 67 | //180° rotation |
Helvis | 1:1adf5dfcc7bb | 68 | void Motion::reverse() { |
Helvis | 1:1adf5dfcc7bb | 69 | |
Helvis | 1:1adf5dfcc7bb | 70 | while(countsLeft < 200 || countsRight > -200) { |
Helvis | 1:1adf5dfcc7bb | 71 | //Count Grenze durch Rechnung bestimmen für 180° Umfang/2 |
Helvis | 1:1adf5dfcc7bb | 72 | countsLeft = counterLeft.read(); |
Helvis | 1:1adf5dfcc7bb | 73 | countsRight = counterRight.read(); |
Helvis | 1:1adf5dfcc7bb | 74 | |
Helvis | 1:1adf5dfcc7bb | 75 | controller.setDesiredSpeedLeft(50.0f); |
Helvis | 1:1adf5dfcc7bb | 76 | controller.setDesiredSpeedRight(-50.0f); |
Helvis | 1:1adf5dfcc7bb | 77 | |
Helvis | 1:1adf5dfcc7bb | 78 | } |
Helvis | 1:1adf5dfcc7bb | 79 | } |
Helvis | 1:1adf5dfcc7bb | 80 |