Helvijs Kiselis / Mbed 2 deprecated MicromousePES2

main.cpp: Sensoren einlesen und Motoren ansteuern

Dependencies:   mbed

Motion.cpp@5:47262622a9bf, 2018-04-16 (annotated)

Committer:
Helvis
Date:
Mon Apr 16 12:44:48 2018 +0000
Revision:
5:47262622a9bf
Parent:
4:e74c06e43485
Child:
8:862bf9225953
testtestest;

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 5:47262622a9bf 13 IRSensor& irSensorC, IRSensor& irSensorR,
Helvis 5:47262622a9bf 14 DigitalOut& enableMotorDriver) :
Helvis 1:1adf5dfcc7bb 15 controller(controller), counterLeft(counterLeft),
Helvis 1:1adf5dfcc7bb 16 counterRight(counterRight), irSensorL(irSensorL),
Helvis 5:47262622a9bf 17 irSensorC(irSensorC), irSensorR(irSensorR),
Helvis 5:47262622a9bf 18 enableMotorDriver(enableMotorDriver) {
Helvis 1:1adf5dfcc7bb 19
Helvis 5:47262622a9bf 20 countsL = 0;
Helvis 5:47262622a9bf 21 countsR = 0;
Helvis 5:47262622a9bf 22 countsLOld = 0;
Helvis 5:47262622a9bf 23 countsROld = 0 ;
Helvis 1:1adf5dfcc7bb 24 }
Helvis 1:1adf5dfcc7bb 25
Helvis 1:1adf5dfcc7bb 26 Motion::~Motion() {}
Helvis 1:1adf5dfcc7bb 27
Helvis 5:47262622a9bf 28 /**
Helvis 5:47262622a9bf 29 * Eine Feldbewegung druchführen
Helvis 5:47262622a9bf 30 */
Helvis 1:1adf5dfcc7bb 31 void Motion::move() {
Helvis 1:1adf5dfcc7bb 32
Helvis 5:47262622a9bf 33 //distanceL = irSensorL.read();
Helvis 5:47262622a9bf 34 //distanceC = irSensorC.read();
Helvis 5:47262622a9bf 35 //distanceR = irSensorR.read();
Helvis 5:47262622a9bf 36
Helvis 5:47262622a9bf 37 while ((countsL - countsLOld) < 1630 || (countsR - countsROld) > -1630) {
Helvis 5:47262622a9bf 38
Helvis 5:47262622a9bf 39 countsL = counterLeft.read();
Helvis 5:47262622a9bf 40 countsR = counterRight.read();
Helvis 1:1adf5dfcc7bb 41
Helvis 5:47262622a9bf 42 controller.setDesiredSpeedLeft(50.0f);
Helvis 5:47262622a9bf 43 controller.setDesiredSpeedRight(-50.0f);
Helvis 5:47262622a9bf 44 enableMotorDriver = 1;
Helvis 5:47262622a9bf 45 }
Helvis 5:47262622a9bf 46 //printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
Helvis 5:47262622a9bf 47
Helvis 5:47262622a9bf 48 stop();
Helvis 5:47262622a9bf 49 enableMotorDriver = 0;
Helvis 5:47262622a9bf 50 countsLOld = countsL + 150;
Helvis 5:47262622a9bf 51 countsROld = countsR - 150;
Helvis 1:1adf5dfcc7bb 52 }
Helvis 5:47262622a9bf 53
Helvis 5:47262622a9bf 54 /**
Helvis 5:47262622a9bf 55 * 90° Rotation nach Links
Helvis 5:47262622a9bf 56 */
Helvis 1:1adf5dfcc7bb 57 void Motion::rotateL() {
Helvis 1:1adf5dfcc7bb 58
Helvis 5:47262622a9bf 59 while ((countsL - countsLOld) > -760 || (countsR - countsROld) > -760) {
Helvis 5:47262622a9bf 60
Helvis 5:47262622a9bf 61 countsL = counterLeft.read();
Helvis 5:47262622a9bf 62 countsR = counterRight.read();
Helvis 5:47262622a9bf 63
Helvis 5:47262622a9bf 64 controller.setDesiredSpeedLeft(-50.0f);
Helvis 5:47262622a9bf 65 controller.setDesiredSpeedRight(-50.0f);
Helvis 5:47262622a9bf 66 enableMotorDriver = 1;
Helvis 5:47262622a9bf 67 }
Helvis 5:47262622a9bf 68
Helvis 5:47262622a9bf 69 stop();
Helvis 5:47262622a9bf 70 enableMotorDriver = 0;
Helvis 5:47262622a9bf 71 countsLOld = countsL - 100;
Helvis 5:47262622a9bf 72 countsROld = countsR - 100;
Helvis 1:1adf5dfcc7bb 73 }
Helvis 1:1adf5dfcc7bb 74
Helvis 5:47262622a9bf 75 /**
Helvis 5:47262622a9bf 76 * 90° Rotation nach Rechts
Helvis 5:47262622a9bf 77 */
Helvis 1:1adf5dfcc7bb 78 void Motion::rotateR() {
Helvis 1:1adf5dfcc7bb 79
Helvis 5:47262622a9bf 80 while ((countsL - countsLOld) < 760 || (countsR - countsROld) < 760) {
Helvis 5:47262622a9bf 81
Helvis 5:47262622a9bf 82 countsL = counterLeft.read();
Helvis 5:47262622a9bf 83 countsR = counterRight.read();
Helvis 5:47262622a9bf 84
Helvis 5:47262622a9bf 85 controller.setDesiredSpeedLeft(50.0f);
Helvis 5:47262622a9bf 86 controller.setDesiredSpeedRight(50.0f);
Helvis 5:47262622a9bf 87 enableMotorDriver = 1;
Helvis 5:47262622a9bf 88 }
Helvis 5:47262622a9bf 89
Helvis 5:47262622a9bf 90 stop();
Helvis 5:47262622a9bf 91 enableMotorDriver = 0;
Helvis 5:47262622a9bf 92 countsLOld = countsL + 100;
Helvis 5:47262622a9bf 93 countsROld = countsR + 100;
Helvis 1:1adf5dfcc7bb 94 }
Helvis 1:1adf5dfcc7bb 95
Helvis 5:47262622a9bf 96 /**
Helvis 5:47262622a9bf 97 * Motor Stop
Helvis 5:47262622a9bf 98 */
Helvis 5:47262622a9bf 99 void Motion::stop() {
Helvis 5:47262622a9bf 100
Helvis 5:47262622a9bf 101 controller.setDesiredSpeedLeft(0.0f);
Helvis 5:47262622a9bf 102 controller.setDesiredSpeedRight(0.0f);
Helvis 5:47262622a9bf 103 }
Helvis 5:47262622a9bf 104 /**
Helvis 5:47262622a9bf 105 * 180° Rotation
Helvis 5:47262622a9bf 106 */
Helvis 5:47262622a9bf 107 void Motion::rotate180() {
Helvis 1:1adf5dfcc7bb 108
Helvis 5:47262622a9bf 109 while ((countsL - countsLOld) > -1560 || (countsR - countsROld) > -1560) {
Helvis 5:47262622a9bf 110
Helvis 5:47262622a9bf 111 countsL = counterLeft.read();
Helvis 5:47262622a9bf 112 countsR = counterRight.read();
Helvis 5:47262622a9bf 113
Helvis 5:47262622a9bf 114 controller.setDesiredSpeedLeft(-50.0f);
Helvis 5:47262622a9bf 115 controller.setDesiredSpeedRight(-50.0f);
Helvis 5:47262622a9bf 116 enableMotorDriver = 1;
Helvis 5:47262622a9bf 117 printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
Helvis 5:47262622a9bf 118 }
Helvis 5:47262622a9bf 119
Helvis 5:47262622a9bf 120 stop();
Helvis 5:47262622a9bf 121 enableMotorDriver = 0;
Helvis 5:47262622a9bf 122 countsLOld = countsL - 150;
Helvis 5:47262622a9bf 123 countsROld = countsR - 150;
Helvis 5:47262622a9bf 124
Helvis 5:47262622a9bf 125 }
Helvis 5:47262622a9bf 126
Helvis 5:47262622a9bf 127 void Motion::test() {
Helvis 5:47262622a9bf 128
Helvis 5:47262622a9bf 129 while (countsL > -1560 || countsR > - 1560) {
Helvis 5:47262622a9bf 130
Helvis 5:47262622a9bf 131 countsL = counterLeft.read();
Helvis 5:47262622a9bf 132 countsR = counterRight.read();
Helvis 5:47262622a9bf 133
Helvis 5:47262622a9bf 134 controller.setDesiredSpeedLeft(-50.0f);
Helvis 5:47262622a9bf 135 controller.setDesiredSpeedRight(-50.0f);
Helvis 5:47262622a9bf 136 enableMotorDriver = 1;
Helvis 5:47262622a9bf 137 printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
Helvis 5:47262622a9bf 138 }
Helvis 5:47262622a9bf 139
Helvis 5:47262622a9bf 140 counterLeft.reset();
Helvis 5:47262622a9bf 141 counterRight.reset();
Helvis 5:47262622a9bf 142 stop();
Helvis 5:47262622a9bf 143 enableMotorDriver = 0;
Helvis 5:47262622a9bf 144 }
Helvis 5:47262622a9bf 145
Helvis 1:1adf5dfcc7bb 146