Helvijs Kiselis / Mbed 2 deprecated MicromousePES2

main.cpp: Sensoren einlesen und Motoren ansteuern

Dependencies:   mbed

Motion.cpp@8:862bf9225953, 2018-04-16 (annotated)

Committer:
luethale
Date:
Mon Apr 16 15:23:42 2018 +0000
Revision:
8:862bf9225953
Parent:
5:47262622a9bf 
Regelung f?r Spurhaltung

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
luethale 8:862bf9225953 33 //distanceL = irSensorL.readL();
luethale 8:862bf9225953 34 //distanceC = irSensorC.readC();
luethale 8:862bf9225953 35 //distanceR = irSensorR.readR();
luethale 8:862bf9225953 36 float korr = 1;
luethale 8:862bf9225953 37 const float kp = 0.28f;
luethale 8:862bf9225953 38 float speedRight = -50.0f;
luethale 8:862bf9225953 39 float speedLeft = 50.0f;
Helvis 5:47262622a9bf 40
Helvis 5:47262622a9bf 41 while ((countsL - countsLOld) < 1630 || (countsR - countsROld) > -1630) {
Helvis 5:47262622a9bf 42
Helvis 5:47262622a9bf 43 countsL = counterLeft.read();
Helvis 5:47262622a9bf 44 countsR = counterRight.read();
luethale 8:862bf9225953 45 distanceL = irSensorL.readL();
luethale 8:862bf9225953 46 distanceR = irSensorR.readR();
luethale 8:862bf9225953 47
luethale 8:862bf9225953 48 korr = (distanceL-distanceR)*kp;
luethale 8:862bf9225953 49
luethale 8:862bf9225953 50 if (korr <= 1){
luethale 8:862bf9225953 51 speedRight = -50.0f * korr;
luethale 8:862bf9225953 52 speedLeft = 50.0f
luethale 8:862bf9225953 53 }
luethale 8:862bf9225953 54 else
luethale 8:862bf9225953 55 speedLeft = 50.0f * -korr;
luethale 8:862bf9225953 56 speedRight = -50.0f;
Helvis 1:1adf5dfcc7bb 57
luethale 8:862bf9225953 58 controller.setDesiredSpeedLeft(speedLeft);
luethale 8:862bf9225953 59 controller.setDesiredSpeedRight(speedRight);
Helvis 5:47262622a9bf 60 enableMotorDriver = 1;
Helvis 5:47262622a9bf 61 }
Helvis 5:47262622a9bf 62 //printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
Helvis 5:47262622a9bf 63
Helvis 5:47262622a9bf 64 stop();
Helvis 5:47262622a9bf 65 enableMotorDriver = 0;
Helvis 5:47262622a9bf 66 countsLOld = countsL + 150;
Helvis 5:47262622a9bf 67 countsROld = countsR - 150;
Helvis 1:1adf5dfcc7bb 68 }
Helvis 5:47262622a9bf 69
Helvis 5:47262622a9bf 70 /**
Helvis 5:47262622a9bf 71 * 90° Rotation nach Links
Helvis 5:47262622a9bf 72 */
Helvis 1:1adf5dfcc7bb 73 void Motion::rotateL() {
Helvis 1:1adf5dfcc7bb 74
Helvis 5:47262622a9bf 75 while ((countsL - countsLOld) > -760 || (countsR - countsROld) > -760) {
Helvis 5:47262622a9bf 76
Helvis 5:47262622a9bf 77 countsL = counterLeft.read();
Helvis 5:47262622a9bf 78 countsR = counterRight.read();
Helvis 5:47262622a9bf 79
Helvis 5:47262622a9bf 80 controller.setDesiredSpeedLeft(-50.0f);
Helvis 5:47262622a9bf 81 controller.setDesiredSpeedRight(-50.0f);
Helvis 5:47262622a9bf 82 enableMotorDriver = 1;
Helvis 5:47262622a9bf 83 }
Helvis 5:47262622a9bf 84
Helvis 5:47262622a9bf 85 stop();
Helvis 5:47262622a9bf 86 enableMotorDriver = 0;
Helvis 5:47262622a9bf 87 countsLOld = countsL - 100;
Helvis 5:47262622a9bf 88 countsROld = countsR - 100;
Helvis 1:1adf5dfcc7bb 89 }
Helvis 1:1adf5dfcc7bb 90
Helvis 5:47262622a9bf 91 /**
Helvis 5:47262622a9bf 92 * 90° Rotation nach Rechts
Helvis 5:47262622a9bf 93 */
Helvis 1:1adf5dfcc7bb 94 void Motion::rotateR() {
Helvis 1:1adf5dfcc7bb 95
Helvis 5:47262622a9bf 96 while ((countsL - countsLOld) < 760 || (countsR - countsROld) < 760) {
Helvis 5:47262622a9bf 97
Helvis 5:47262622a9bf 98 countsL = counterLeft.read();
Helvis 5:47262622a9bf 99 countsR = counterRight.read();
Helvis 5:47262622a9bf 100
Helvis 5:47262622a9bf 101 controller.setDesiredSpeedLeft(50.0f);
Helvis 5:47262622a9bf 102 controller.setDesiredSpeedRight(50.0f);
Helvis 5:47262622a9bf 103 enableMotorDriver = 1;
Helvis 5:47262622a9bf 104 }
Helvis 5:47262622a9bf 105
Helvis 5:47262622a9bf 106 stop();
Helvis 5:47262622a9bf 107 enableMotorDriver = 0;
Helvis 5:47262622a9bf 108 countsLOld = countsL + 100;
Helvis 5:47262622a9bf 109 countsROld = countsR + 100;
Helvis 1:1adf5dfcc7bb 110 }
Helvis 1:1adf5dfcc7bb 111
Helvis 5:47262622a9bf 112 /**
Helvis 5:47262622a9bf 113 * Motor Stop
Helvis 5:47262622a9bf 114 */
Helvis 5:47262622a9bf 115 void Motion::stop() {
Helvis 5:47262622a9bf 116
Helvis 5:47262622a9bf 117 controller.setDesiredSpeedLeft(0.0f);
Helvis 5:47262622a9bf 118 controller.setDesiredSpeedRight(0.0f);
Helvis 5:47262622a9bf 119 }
Helvis 5:47262622a9bf 120 /**
Helvis 5:47262622a9bf 121 * 180° Rotation
Helvis 5:47262622a9bf 122 */
Helvis 5:47262622a9bf 123 void Motion::rotate180() {
Helvis 1:1adf5dfcc7bb 124
Helvis 5:47262622a9bf 125 while ((countsL - countsLOld) > -1560 || (countsR - countsROld) > -1560) {
Helvis 5:47262622a9bf 126
Helvis 5:47262622a9bf 127 countsL = counterLeft.read();
Helvis 5:47262622a9bf 128 countsR = counterRight.read();
Helvis 5:47262622a9bf 129
Helvis 5:47262622a9bf 130 controller.setDesiredSpeedLeft(-50.0f);
Helvis 5:47262622a9bf 131 controller.setDesiredSpeedRight(-50.0f);
Helvis 5:47262622a9bf 132 enableMotorDriver = 1;
Helvis 5:47262622a9bf 133 printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
Helvis 5:47262622a9bf 134 }
Helvis 5:47262622a9bf 135
Helvis 5:47262622a9bf 136 stop();
Helvis 5:47262622a9bf 137 enableMotorDriver = 0;
Helvis 5:47262622a9bf 138 countsLOld = countsL - 150;
Helvis 5:47262622a9bf 139 countsROld = countsR - 150;
Helvis 5:47262622a9bf 140
Helvis 5:47262622a9bf 141 }
Helvis 5:47262622a9bf 142
Helvis 5:47262622a9bf 143 void Motion::test() {
Helvis 5:47262622a9bf 144
Helvis 5:47262622a9bf 145 while (countsL > -1560 || countsR > - 1560) {
Helvis 5:47262622a9bf 146
Helvis 5:47262622a9bf 147 countsL = counterLeft.read();
Helvis 5:47262622a9bf 148 countsR = counterRight.read();
Helvis 5:47262622a9bf 149
Helvis 5:47262622a9bf 150 controller.setDesiredSpeedLeft(-50.0f);
Helvis 5:47262622a9bf 151 controller.setDesiredSpeedRight(-50.0f);
Helvis 5:47262622a9bf 152 enableMotorDriver = 1;
Helvis 5:47262622a9bf 153 printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
Helvis 5:47262622a9bf 154 }
Helvis 5:47262622a9bf 155
Helvis 5:47262622a9bf 156 counterLeft.reset();
Helvis 5:47262622a9bf 157 counterRight.reset();
Helvis 5:47262622a9bf 158 stop();
Helvis 5:47262622a9bf 159 enableMotorDriver = 0;
Helvis 5:47262622a9bf 160 }
Helvis 5:47262622a9bf 161
Helvis 1:1adf5dfcc7bb 162