Antonia Baumgartner / Mbed 2 deprecated YB_copy

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Versuch21 by Antonia Baumgartner

Classes/Motion.cpp@2:efa9a78591da, 2018-04-25 (annotated)

Committer:
baumgant
Date:
Wed Apr 25 11:16:16 2018 +0000
Revision:
2:efa9a78591da
Parent:
1:6ef5bc60e69c
Child:
3:f44ef28cfb2d
PES2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
baumgant 0:b886f13e4ac6 1 #include "Motion.h"
baumgant 0:b886f13e4ac6 2 #include "Controller.h"
baumgant 0:b886f13e4ac6 3 #include "EncoderCounter.h"
baumgant 0:b886f13e4ac6 4 #include "Spurhaltung.h"
mrbb2 1:6ef5bc60e69c 5 #include "IRSensorGF.h"
mrbb2 1:6ef5bc60e69c 6 #include "IRSensorG.h"
mrbb2 1:6ef5bc60e69c 7 #include "IRSensorK.h"
mrbb2 1:6ef5bc60e69c 8 #include "IRSensorZ.h"
baumgant 0:b886f13e4ac6 9
baumgant 2:efa9a78591da 10 // Left + = Rückwärts Left - = Vorwärts
baumgant 2:efa9a78591da 11 // Right + = Vorwärts Right - = Rückwärts
baumgant 0:b886f13e4ac6 12
baumgant 0:b886f13e4ac6 13 using namespace std;
baumgant 0:b886f13e4ac6 14
mrbb2 1:6ef5bc60e69c 15 Motion::Motion(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, Spurhaltung& spurhaltung, IRSensorGF& Sensor1, IRSensorG& Sensor2, IRSensorG& Sensor3, IRSensorK& Sensor4, IRSensorK& Sensor5, IRSensorZ& Sensor6):
mrbb2 1:6ef5bc60e69c 16 counterLeft(counterLeft), counterRight(counterRight), controller(controller), spurhaltung(spurhaltung), Sensor1(Sensor1), Sensor2(Sensor2), Sensor3(Sensor3), Sensor4(Sensor4), Sensor5(Sensor5), Sensor6(Sensor6)
baumgant 0:b886f13e4ac6 17 {
mrbb2 1:6ef5bc60e69c 18
baumgant 0:b886f13e4ac6 19 }
baumgant 0:b886f13e4ac6 20
baumgant 0:b886f13e4ac6 21 Motion::~Motion() {}
baumgant 0:b886f13e4ac6 22
baumgant 2:efa9a78591da 23 //------------------------------------------------------------------------------
mrbb2 1:6ef5bc60e69c 24
mrbb2 1:6ef5bc60e69c 25 void Motion::drehenl90()
mrbb2 1:6ef5bc60e69c 26 {
mrbb2 1:6ef5bc60e69c 27 controller.resetCounter();
mrbb2 1:6ef5bc60e69c 28 int lastCountRight = counterRight.read();
mrbb2 1:6ef5bc60e69c 29
baumgant 2:efa9a78591da 30 while(counterRight.read()-lastCountRight>(-1.35*1260)) {
baumgant 2:efa9a78591da 31 controller.setDesiredSpeedLeft(150.0f); //Drehzahl in [rpm]
baumgant 2:efa9a78591da 32 controller.setDesiredSpeedRight(150.0f);
baumgant 2:efa9a78591da 33
baumgant 2:efa9a78591da 34
mrbb2 1:6ef5bc60e69c 35 }
mrbb2 1:6ef5bc60e69c 36
mrbb2 1:6ef5bc60e69c 37 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
mrbb2 1:6ef5bc60e69c 38 controller.setDesiredSpeedRight(0.0f);
mrbb2 1:6ef5bc60e69c 39
mrbb2 1:6ef5bc60e69c 40 controller.resetCounter();
mrbb2 1:6ef5bc60e69c 41 int lastCountLeft = counterLeft.read();
mrbb2 1:6ef5bc60e69c 42
baumgant 2:efa9a78591da 43 while(counterLeft.read()-lastCountLeft>(-2.094*1260)) {
baumgant 2:efa9a78591da 44 controller.setDesiredSpeedLeft(-spurhaltung.speedr()); //Drehzahl in [rpm]
baumgant 2:efa9a78591da 45 controller.setDesiredSpeedRight(spurhaltung.speedl());
mrbb2 1:6ef5bc60e69c 46 spurhaltung.speedl();
mrbb2 1:6ef5bc60e69c 47 spurhaltung.speedr();
baumgant 2:efa9a78591da 48
mrbb2 1:6ef5bc60e69c 49 }
mrbb2 1:6ef5bc60e69c 50
mrbb2 1:6ef5bc60e69c 51 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
mrbb2 1:6ef5bc60e69c 52 controller.setDesiredSpeedRight(0.0f);
mrbb2 1:6ef5bc60e69c 53
mrbb2 1:6ef5bc60e69c 54 }
mrbb2 1:6ef5bc60e69c 55
baumgant 2:efa9a78591da 56 //------------------------------------------------------------------------------
baumgant 2:efa9a78591da 57
baumgant 2:efa9a78591da 58 void Motion::gerade()
baumgant 2:efa9a78591da 59 {
baumgant 2:efa9a78591da 60
baumgant 2:efa9a78591da 61 controller.resetCounter();
baumgant 2:efa9a78591da 62 int lastCountLeft = counterLeft.read();
baumgant 2:efa9a78591da 63
baumgant 2:efa9a78591da 64 while(counterLeft.read()-lastCountLeft>(-2.094*1260)) {
baumgant 2:efa9a78591da 65 controller.setDesiredSpeedLeft(-spurhaltung.speedr()); //Drehzahl in [rpm]
baumgant 2:efa9a78591da 66 controller.setDesiredSpeedRight(spurhaltung.speedl());
baumgant 2:efa9a78591da 67 spurhaltung.speedl();
baumgant 2:efa9a78591da 68 spurhaltung.speedr();
baumgant 2:efa9a78591da 69 }
baumgant 2:efa9a78591da 70
baumgant 2:efa9a78591da 71 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
baumgant 2:efa9a78591da 72 controller.setDesiredSpeedRight(0.0f);
baumgant 2:efa9a78591da 73 }
baumgant 2:efa9a78591da 74
baumgant 2:efa9a78591da 75 //------------------------------------------------------------------------------
baumgant 2:efa9a78591da 76
mrbb2 1:6ef5bc60e69c 77 void Motion::drehenr90()
mrbb2 1:6ef5bc60e69c 78 {
mrbb2 1:6ef5bc60e69c 79 controller.resetCounter();
mrbb2 1:6ef5bc60e69c 80 int lastCountLeft = counterLeft.read();
mrbb2 1:6ef5bc60e69c 81
baumgant 2:efa9a78591da 82 while(counterLeft.read()-lastCountLeft<(1.35*1260)) {
baumgant 2:efa9a78591da 83 controller.setDesiredSpeedLeft(-150.0f); //Drehzahl in [rpm]
baumgant 2:efa9a78591da 84 controller.setDesiredSpeedRight(-150.0f);
baumgant 2:efa9a78591da 85
mrbb2 1:6ef5bc60e69c 86 }
mrbb2 1:6ef5bc60e69c 87
mrbb2 1:6ef5bc60e69c 88 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
mrbb2 1:6ef5bc60e69c 89 controller.setDesiredSpeedRight(0.0f);
mrbb2 1:6ef5bc60e69c 90
mrbb2 1:6ef5bc60e69c 91 controller.resetCounter();
mrbb2 1:6ef5bc60e69c 92 lastCountLeft = counterLeft.read();
mrbb2 1:6ef5bc60e69c 93
baumgant 2:efa9a78591da 94 while(counterLeft.read()-lastCountLeft<(-2.094*1260)) {
baumgant 2:efa9a78591da 95 controller.setDesiredSpeedLeft(-spurhaltung.speedr()); //Drehzahl in [rpm]
baumgant 2:efa9a78591da 96 controller.setDesiredSpeedRight(spurhaltung.speedl());
mrbb2 1:6ef5bc60e69c 97 spurhaltung.speedl();
mrbb2 1:6ef5bc60e69c 98 spurhaltung.speedr();
baumgant 2:efa9a78591da 99
mrbb2 1:6ef5bc60e69c 100 }
mrbb2 1:6ef5bc60e69c 101
mrbb2 1:6ef5bc60e69c 102 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
mrbb2 1:6ef5bc60e69c 103 controller.setDesiredSpeedRight(0.0f);
mrbb2 1:6ef5bc60e69c 104 }
mrbb2 1:6ef5bc60e69c 105
baumgant 2:efa9a78591da 106 //------------------------------------------------------------------------------
baumgant 2:efa9a78591da 107
mrbb2 1:6ef5bc60e69c 108 void Motion::drehen180()
mrbb2 1:6ef5bc60e69c 109 {
mrbb2 1:6ef5bc60e69c 110 controller.resetCounter();
mrbb2 1:6ef5bc60e69c 111 int lastCountRight = counterRight.read();
mrbb2 1:6ef5bc60e69c 112
baumgant 2:efa9a78591da 113 while(counterRight.read()-lastCountRight>(-2.7*1260)) {
mrbb2 1:6ef5bc60e69c 114 controller.setDesiredSpeedLeft(150.0f); //Drehzahl in [rpm]
mrbb2 1:6ef5bc60e69c 115 controller.setDesiredSpeedRight(150.0f);
baumgant 2:efa9a78591da 116
baumgant 2:efa9a78591da 117
mrbb2 1:6ef5bc60e69c 118 }
mrbb2 1:6ef5bc60e69c 119
mrbb2 1:6ef5bc60e69c 120 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
mrbb2 1:6ef5bc60e69c 121 controller.setDesiredSpeedRight(0.0f);
mrbb2 1:6ef5bc60e69c 122 }
baumgant 0:b886f13e4ac6 123
baumgant 2:efa9a78591da 124 //------------------------------------------------------------------------------
baumgant 2:efa9a78591da 125
baumgant 0:b886f13e4ac6 126 void Motion::switching(int D)
baumgant 0:b886f13e4ac6 127 {
baumgant 0:b886f13e4ac6 128 switch(D) {
baumgant 0:b886f13e4ac6 129 case 1:
mrbb2 1:6ef5bc60e69c 130 // Left
mrbb2 1:6ef5bc60e69c 131 drehenl90();
baumgant 0:b886f13e4ac6 132 // Forward
mrbb2 1:6ef5bc60e69c 133 //gerade();
baumgant 0:b886f13e4ac6 134 break;
baumgant 0:b886f13e4ac6 135
baumgant 0:b886f13e4ac6 136 case 2:
mrbb2 1:6ef5bc60e69c 137 // Forward
mrbb2 1:6ef5bc60e69c 138 gerade();
baumgant 0:b886f13e4ac6 139 break;
baumgant 0:b886f13e4ac6 140
baumgant 0:b886f13e4ac6 141 case 3:
mrbb2 1:6ef5bc60e69c 142 // Right
mrbb2 1:6ef5bc60e69c 143 drehenr90();
baumgant 0:b886f13e4ac6 144 // Forward
mrbb2 1:6ef5bc60e69c 145 gerade();
baumgant 0:b886f13e4ac6 146 break;
baumgant 0:b886f13e4ac6 147
mrbb2 1:6ef5bc60e69c 148 case 4:
mrbb2 1:6ef5bc60e69c 149 // 180 Drehen
mrbb2 1:6ef5bc60e69c 150 drehen180();
mrbb2 1:6ef5bc60e69c 151 break;
baumgant 0:b886f13e4ac6 152
baumgant 0:b886f13e4ac6 153 default:
baumgant 0:b886f13e4ac6 154 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
baumgant 0:b886f13e4ac6 155 controller.setDesiredSpeedRight(0.0f);
baumgant 0:b886f13e4ac6 156 }
mrbb2 1:6ef5bc60e69c 157
baumgant 2:efa9a78591da 158 //------------------------------------------------------------------------------
mrbb2 1:6ef5bc60e69c 159
mrbb2 1:6ef5bc60e69c 160 /*void Motion::switching()
mrbb2 1:6ef5bc60e69c 161 {
mrbb2 1:6ef5bc60e69c 162 if(Sensor4.read() == 0) {
mrbb2 1:6ef5bc60e69c 163 // Left
mrbb2 1:6ef5bc60e69c 164 drehenl90();
mrbb2 1:6ef5bc60e69c 165 // Forward
mrbb2 1:6ef5bc60e69c 166 gerade();
mrbb2 1:6ef5bc60e69c 167 } else if (Sensor1.read() == 0) {
mrbb2 1:6ef5bc60e69c 168 // Forward
mrbb2 1:6ef5bc60e69c 169 gerade();
mrbb2 1:6ef5bc60e69c 170 } else if (Sensor5.read() == 0) {
mrbb2 1:6ef5bc60e69c 171 // Right
mrbb2 1:6ef5bc60e69c 172 drehenr90();
mrbb2 1:6ef5bc60e69c 173 // Forward
mrbb2 1:6ef5bc60e69c 174 gerade();
mrbb2 1:6ef5bc60e69c 175 } else {
mrbb2 1:6ef5bc60e69c 176 // 180 Drehen
mrbb2 1:6ef5bc60e69c 177 drehen180();
mrbb2 1:6ef5bc60e69c 178 // Forward
mrbb2 1:6ef5bc60e69c 179 gerade();
mrbb2 1:6ef5bc60e69c 180
mrbb2 1:6ef5bc60e69c 181 }*/
baumgant 0:b886f13e4ac6 182
baumgant 0:b886f13e4ac6 183 }