Helvijs Kiselis / Mbed 2 deprecated MicromousePES2

main.cpp: Sensoren einlesen und Motoren ansteuern

Dependencies:   mbed

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?

UserRevisionLine numberNew 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